From 195053d034b2bca63db1f72be14bff9b2c7b1916 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Fri, 17 Mar 2017 01:13:42 -0400 Subject: bump rawdraw to newest version. --- Makefile | 8 +- calibrate.c | 2 +- calibrate_client.c | 2 +- data_recorder.c | 2 +- redist/CNFGFunctions.c | 272 ++++++++++++++++++++++++++++++++++++++++ redist/CNFGFunctions.h | 59 +++++++++ redist/CNFGNullDriver.c | 74 +++++++++++ redist/CNFGRasterizer.h | 247 ++++++++++++++++++++++++++++++++++++ redist/CNFGWinDriver.c | 289 ++++++++++++++++++++++++++++++++++++++++++ redist/CNFGXDriver.c | 304 +++++++++++++++++++++++++++++++++++++++++++++ redist/DrawFunctions.c | 277 ----------------------------------------- redist/DrawFunctions.h | 55 -------- redist/RawDrawNull.c | 74 ----------- redist/RawDrawRasterizer.c | 238 ----------------------------------- redist/WinDriver.c | 231 ---------------------------------- redist/XDriver.c | 290 ------------------------------------------ test.c | 2 +- 17 files changed, 1253 insertions(+), 1173 deletions(-) create mode 100644 redist/CNFGFunctions.c create mode 100644 redist/CNFGFunctions.h create mode 100644 redist/CNFGNullDriver.c create mode 100644 redist/CNFGRasterizer.h create mode 100644 redist/CNFGWinDriver.c create mode 100644 redist/CNFGXDriver.c delete mode 100644 redist/DrawFunctions.c delete mode 100644 redist/DrawFunctions.h delete mode 100644 redist/RawDrawNull.c delete mode 100644 redist/RawDrawRasterizer.c delete mode 100644 redist/WinDriver.c delete mode 100644 redist/XDriver.c diff --git a/Makefile b/Makefile index 03f37d6..a642877 100644 --- a/Makefile +++ b/Makefile @@ -13,14 +13,14 @@ UNAME=$(shell uname) # Mac OSX ifeq ($(UNAME), Darwin) -DRAWFUNCTIONS=redist/DrawFunctions.c redist/RawDrawNull.c -GRAPHICS_LOFI:=redist/DrawFunctions.o redist/RawDrawNull.o +DRAWFUNCTIONS=redist/CNFGFunctions.c redist/CNFGNullDriver.c +GRAPHICS_LOFI:=redist/CNFGFunctions.o redist/CNFGNullDriver.o # Linux / FreeBSD else LDFLAGS:=$(LDFLAGS) -lX11 -DRAWFUNCTIONS=redist/DrawFunctions.c redist/XDriver.c -GRAPHICS_LOFI:=redist/DrawFunctions.o redist/XDriver.o +DRAWFUNCTIONS=redist/CNFGFunctions.c redist/CNFGXDriver.c +GRAPHICS_LOFI:=redist/CNFGFunctions.o redist/CNFGXDriver.o endif diff --git a/calibrate.c b/calibrate.c index 9633a77..3d83baf 100644 --- a/calibrate.c +++ b/calibrate.c @@ -7,7 +7,7 @@ #include #include #include "src/survive_cal.h" -#include +#include #include "src/survive_config.h" diff --git a/calibrate_client.c b/calibrate_client.c index f28520b..5e31cfb 100644 --- a/calibrate_client.c +++ b/calibrate_client.c @@ -8,7 +8,7 @@ #include #include #include "src/survive_cal.h" -#include +#include #include "src/survive_config.h" diff --git a/data_recorder.c b/data_recorder.c index 5504d42..630598a 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -7,7 +7,7 @@ #include #include #include -#include +#include struct SurviveContext * ctx; diff --git a/redist/CNFGFunctions.c b/redist/CNFGFunctions.c new file mode 100644 index 0000000..947456f --- /dev/null +++ b/redist/CNFGFunctions.c @@ -0,0 +1,272 @@ +/* +Copyright (c) 2010, 2011 Charles Lohr + +Permission is hereby granted, free of charge, to any person obtaining +a copy of this software and associated documentation files (the +"Software"), to deal in the Software without restriction, including +without limitation the rights to use, copy, modify, merge, publish, +distribute, sublicense, and/or sell copies of the Software, and to +permit persons to whom the Software is furnished to do so, subject to +the following conditions: + +The above copyright notice and this permission notice shall be included +in all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. +IN NO EVENT SHALL THE X CONSORTIUM BE LIABLE FOR ANY CLAIM, DAMAGES OR +OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, +ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR +OTHER DEALINGS IN THE SOFTWARE. +*/ + +#include "CNFGFunctions.h" +#include + +int CNFGPenX, CNFGPenY; +uint32_t CNFGBGColor; +uint32_t CNFGLastColor; +uint32_t CNFGDialogColor; //background for boxes +const unsigned short FontCharMap[256] = { + 65535, 0, 10, 20, 32, 44, 56, 68, 70, 65535, 65535, 80, 92, 65535, 104, 114, + 126, 132, 138, 148, 156, 166, 180, 188, 200, 206, 212, 218, 224, 228, 238, 244, + 65535, 250, 254, 258, 266, 278, 288, 302, 304, 310, 316, 324, 328, 226, 252, 330, + 332, 342, 348, 358, 366, 372, 382, 392, 400, 410, 420, 424, 428, 262, 432, 436, + 446, 460, 470, 486, 496, 508, 516, 522, 536, 542, 548, 556, 568, 572, 580, 586, + 598, 608, 622, 634, 644, 648, 654, 662, 670, 682, 692, 700, 706, 708, 492, 198, + 714, 716, 726, 734, 742, 750, 760, 768, 782, 790, 794, 802, 204, 810, 820, 384, + 828, 836, 844, 850, 860, 864, 872, 880, 890, 894, 902, 908, 916, 920, 928, 934, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 944, 948, 960, 970, 986, 996, 1000, 1016, 1020, 1026, 1034, 1042, 364, 1046, 1056, 1058, + 1066, 1072, 1080, 932, 1092, 1100, 68, 1110, 1114, 1116, 1124, 1132, 1142, 1154, 1170, 1180, + 1192, 1204, 1218, 1234, 1248, 1264, 1276, 1290, 1300, 1310, 1322, 1334, 1342, 1350, 1360, 1370, + 1384, 1396, 1406, 1416, 1428, 1442, 1454, 1458, 1472, 1480, 1488, 1498, 1508, 1520, 1530, 1544, + 1556, 1568, 1582, 1598, 1612, 1628, 1642, 1654, 1666, 1678, 1692, 1706, 1710, 1714, 1720, 1726, + 1738, 1752, 1762, 1772, 1784, 1798, 1810, 1816, 1826, 1836, 1846, 1858, 1870, 1880, 1890, 65535, }; + +const unsigned char FontCharData[1902] = { + 0x00, 0x01, 0x20, 0x21, 0x03, 0x23, 0x23, 0x14, 0x14, 0x83, 0x00, 0x01, 0x20, 0x21, 0x04, 0x24, + 0x24, 0x13, 0x13, 0x84, 0x01, 0x21, 0x21, 0x23, 0x23, 0x14, 0x14, 0x03, 0x03, 0x01, 0x11, 0x92, + 0x11, 0x22, 0x22, 0x23, 0x23, 0x14, 0x14, 0x03, 0x03, 0x02, 0x02, 0x91, 0x01, 0x21, 0x21, 0x23, + 0x23, 0x01, 0x03, 0x21, 0x03, 0x01, 0x12, 0x94, 0x03, 0x23, 0x13, 0x14, 0x23, 0x22, 0x22, 0x11, + 0x11, 0x02, 0x02, 0x83, 0x12, 0x92, 0x12, 0x12, 0x01, 0x21, 0x21, 0x23, 0x23, 0x03, 0x03, 0x81, + 0x03, 0x21, 0x21, 0x22, 0x21, 0x11, 0x03, 0x14, 0x14, 0x23, 0x23, 0x92, 0x01, 0x10, 0x10, 0x21, + 0x21, 0x12, 0x12, 0x01, 0x12, 0x14, 0x03, 0xa3, 0x02, 0x03, 0x03, 0x13, 0x02, 0x12, 0x13, 0x10, + 0x10, 0xa1, 0x01, 0x23, 0x03, 0x21, 0x02, 0x11, 0x11, 0x22, 0x22, 0x13, 0x13, 0x82, 0x00, 0x22, + 0x22, 0x04, 0x04, 0x80, 0x20, 0x02, 0x02, 0x24, 0x24, 0xa0, 0x01, 0x10, 0x10, 0x21, 0x10, 0x14, + 0x14, 0x03, 0x14, 0xa3, 0x00, 0x03, 0x04, 0x04, 0x20, 0x23, 0x24, 0xa4, 0x00, 0x20, 0x00, 0x02, + 0x02, 0x22, 0x10, 0x14, 0x20, 0xa4, 0x01, 0x21, 0x21, 0x23, 0x23, 0x03, 0x03, 0x01, 0x20, 0x10, + 0x10, 0x14, 0x14, 0x84, 0x03, 0x23, 0x23, 0x24, 0x24, 0x04, 0x04, 0x83, 0x01, 0x10, 0x10, 0x21, + 0x10, 0x14, 0x14, 0x03, 0x14, 0x23, 0x04, 0xa4, 0x01, 0x10, 0x21, 0x10, 0x10, 0x94, 0x03, 0x14, + 0x23, 0x14, 0x10, 0x94, 0x02, 0x22, 0x22, 0x11, 0x22, 0x93, 0x02, 0x22, 0x02, 0x11, 0x02, 0x93, + 0x01, 0x02, 0x02, 0xa2, 0x02, 0x22, 0x22, 0x11, 0x11, 0x02, 0x02, 0x13, 0x13, 0xa2, 0x11, 0x22, + 0x22, 0x02, 0x02, 0x91, 0x02, 0x13, 0x13, 0x22, 0x22, 0x82, 0x10, 0x13, 0x14, 0x94, 0x10, 0x01, + 0x20, 0x91, 0x10, 0x14, 0x20, 0x24, 0x01, 0x21, 0x03, 0xa3, 0x21, 0x10, 0x10, 0x01, 0x01, 0x23, + 0x23, 0x14, 0x14, 0x03, 0x10, 0x94, 0x00, 0x01, 0x23, 0x24, 0x04, 0x03, 0x03, 0x21, 0x21, 0xa0, + 0x21, 0x10, 0x10, 0x01, 0x01, 0x12, 0x12, 0x03, 0x03, 0x14, 0x14, 0x23, 0x02, 0xa4, 0x10, 0x91, + 0x10, 0x01, 0x01, 0x03, 0x03, 0x94, 0x10, 0x21, 0x21, 0x23, 0x23, 0x94, 0x01, 0x23, 0x11, 0x13, + 0x21, 0x03, 0x02, 0xa2, 0x02, 0x22, 0x11, 0x93, 0x04, 0x93, 0x03, 0xa1, 0x00, 0x20, 0x20, 0x24, + 0x24, 0x04, 0x04, 0x00, 0x12, 0x92, 0x01, 0x10, 0x10, 0x14, 0x04, 0xa4, 0x01, 0x10, 0x10, 0x21, + 0x21, 0x22, 0x22, 0x04, 0x04, 0xa4, 0x00, 0x20, 0x20, 0x24, 0x24, 0x04, 0x12, 0xa2, 0x00, 0x02, + 0x02, 0x22, 0x20, 0xa4, 0x20, 0x00, 0x00, 0x02, 0x02, 0x22, 0x22, 0x24, 0x24, 0x84, 0x20, 0x02, + 0x02, 0x22, 0x22, 0x24, 0x24, 0x04, 0x04, 0x82, 0x00, 0x20, 0x20, 0x21, 0x21, 0x12, 0x12, 0x94, + 0x00, 0x04, 0x00, 0x20, 0x20, 0x24, 0x04, 0x24, 0x02, 0xa2, 0x00, 0x02, 0x02, 0x22, 0x22, 0x20, + 0x20, 0x00, 0x22, 0x84, 0x11, 0x11, 0x13, 0x93, 0x11, 0x11, 0x13, 0x84, 0x20, 0x02, 0x02, 0xa4, + 0x00, 0x22, 0x22, 0x84, 0x01, 0x10, 0x10, 0x21, 0x21, 0x12, 0x12, 0x13, 0x14, 0x94, 0x21, 0x01, + 0x01, 0x04, 0x04, 0x24, 0x24, 0x22, 0x22, 0x12, 0x12, 0x13, 0x13, 0xa3, 0x04, 0x01, 0x01, 0x10, + 0x10, 0x21, 0x21, 0x24, 0x02, 0xa2, 0x00, 0x04, 0x04, 0x14, 0x14, 0x23, 0x23, 0x12, 0x12, 0x02, + 0x12, 0x21, 0x21, 0x10, 0x10, 0x80, 0x23, 0x14, 0x14, 0x03, 0x03, 0x01, 0x01, 0x10, 0x10, 0xa1, + 0x00, 0x10, 0x10, 0x21, 0x21, 0x23, 0x23, 0x14, 0x14, 0x04, 0x04, 0x80, 0x00, 0x04, 0x04, 0x24, + 0x00, 0x20, 0x02, 0x92, 0x00, 0x04, 0x00, 0x20, 0x02, 0x92, 0x21, 0x10, 0x10, 0x01, 0x01, 0x03, + 0x03, 0x14, 0x14, 0x23, 0x23, 0x22, 0x22, 0x92, 0x00, 0x04, 0x20, 0x24, 0x02, 0xa2, 0x00, 0x20, + 0x10, 0x14, 0x04, 0xa4, 0x00, 0x20, 0x20, 0x23, 0x23, 0x14, 0x14, 0x83, 0x00, 0x04, 0x02, 0x12, + 0x12, 0x21, 0x21, 0x20, 0x12, 0x23, 0x23, 0xa4, 0x00, 0x04, 0x04, 0xa4, 0x04, 0x00, 0x00, 0x11, + 0x11, 0x20, 0x20, 0xa4, 0x04, 0x00, 0x00, 0x22, 0x20, 0xa4, 0x01, 0x10, 0x10, 0x21, 0x21, 0x23, + 0x23, 0x14, 0x14, 0x03, 0x03, 0x81, 0x00, 0x04, 0x00, 0x10, 0x10, 0x21, 0x21, 0x12, 0x12, 0x82, + 0x01, 0x10, 0x10, 0x21, 0x21, 0x23, 0x23, 0x14, 0x14, 0x03, 0x03, 0x01, 0x04, 0x93, 0x00, 0x04, + 0x00, 0x10, 0x10, 0x21, 0x21, 0x12, 0x12, 0x02, 0x02, 0xa4, 0x21, 0x10, 0x10, 0x01, 0x01, 0x23, + 0x23, 0x14, 0x14, 0x83, 0x00, 0x20, 0x10, 0x94, 0x00, 0x04, 0x04, 0x24, 0x24, 0xa0, 0x00, 0x03, + 0x03, 0x14, 0x14, 0x23, 0x23, 0xa0, 0x00, 0x04, 0x04, 0x24, 0x14, 0x13, 0x24, 0xa0, 0x00, 0x01, + 0x01, 0x23, 0x23, 0x24, 0x04, 0x03, 0x03, 0x21, 0x21, 0xa0, 0x00, 0x01, 0x01, 0x12, 0x12, 0x14, + 0x12, 0x21, 0x21, 0xa0, 0x00, 0x20, 0x20, 0x02, 0x02, 0x04, 0x04, 0xa4, 0x10, 0x00, 0x00, 0x04, + 0x04, 0x94, 0x01, 0xa3, 0x10, 0x20, 0x20, 0x24, 0x24, 0x94, 0x00, 0x91, 0x02, 0x04, 0x04, 0x24, + 0x24, 0x22, 0x23, 0x12, 0x12, 0x82, 0x00, 0x04, 0x04, 0x24, 0x24, 0x22, 0x22, 0x82, 0x24, 0x04, + 0x04, 0x03, 0x03, 0x12, 0x12, 0xa2, 0x20, 0x24, 0x24, 0x04, 0x04, 0x02, 0x02, 0xa2, 0x24, 0x04, + 0x04, 0x02, 0x02, 0x22, 0x22, 0x23, 0x23, 0x93, 0x04, 0x01, 0x02, 0x12, 0x01, 0x10, 0x10, 0xa1, + 0x23, 0x12, 0x12, 0x03, 0x03, 0x14, 0x14, 0x23, 0x23, 0x24, 0x24, 0x15, 0x15, 0x84, 0x00, 0x04, + 0x03, 0x12, 0x12, 0x23, 0x23, 0xa4, 0x11, 0x11, 0x12, 0x94, 0x22, 0x22, 0x23, 0x24, 0x24, 0x15, + 0x15, 0x84, 0x00, 0x04, 0x03, 0x13, 0x13, 0x22, 0x13, 0xa4, 0x02, 0x04, 0x02, 0x13, 0x12, 0x14, + 0x12, 0x23, 0x23, 0xa4, 0x02, 0x04, 0x03, 0x12, 0x12, 0x23, 0x23, 0xa4, 0x02, 0x05, 0x04, 0x24, + 0x24, 0x22, 0x22, 0x82, 0x02, 0x04, 0x04, 0x24, 0x25, 0x22, 0x22, 0x82, 0x02, 0x04, 0x03, 0x12, + 0x12, 0xa2, 0x22, 0x02, 0x02, 0x03, 0x03, 0x23, 0x23, 0x24, 0x24, 0x84, 0x11, 0x14, 0x02, 0xa2, + 0x02, 0x04, 0x04, 0x14, 0x14, 0x23, 0x24, 0xa2, 0x02, 0x03, 0x03, 0x14, 0x14, 0x23, 0x23, 0xa2, + 0x02, 0x03, 0x03, 0x14, 0x14, 0x12, 0x13, 0x24, 0x24, 0xa2, 0x02, 0x24, 0x04, 0xa2, 0x02, 0x03, + 0x03, 0x14, 0x22, 0x23, 0x23, 0x85, 0x02, 0x22, 0x22, 0x04, 0x04, 0xa4, 0x20, 0x10, 0x10, 0x14, + 0x14, 0x24, 0x12, 0x82, 0x10, 0x11, 0x13, 0x94, 0x00, 0x10, 0x10, 0x14, 0x14, 0x04, 0x12, 0xa2, + 0x01, 0x10, 0x10, 0x11, 0x11, 0xa0, 0x03, 0x04, 0x04, 0x24, 0x24, 0x23, 0x23, 0x12, 0x12, 0x83, + 0x10, 0x10, 0x11, 0x94, 0x21, 0x10, 0x10, 0x01, 0x01, 0x02, 0x02, 0x13, 0x13, 0x22, 0x10, 0x93, + 0x11, 0x00, 0x00, 0x04, 0x04, 0x24, 0x24, 0x23, 0x02, 0x92, 0x01, 0x02, 0x11, 0x21, 0x22, 0x23, + 0x03, 0x13, 0x02, 0x11, 0x11, 0x22, 0x22, 0x13, 0x13, 0x82, 0x00, 0x11, 0x11, 0x20, 0x11, 0x14, + 0x02, 0x22, 0x03, 0xa3, 0x10, 0x12, 0x13, 0x95, 0x20, 0x00, 0x00, 0x02, 0x02, 0x11, 0x11, 0x22, + 0x02, 0x13, 0x13, 0x22, 0x22, 0x24, 0x24, 0x84, 0x00, 0x00, 0x20, 0xa0, 0x20, 0x10, 0x10, 0x11, + 0x11, 0xa1, 0x10, 0x21, 0x20, 0x21, 0x21, 0x11, 0x11, 0x90, 0x11, 0x02, 0x02, 0x13, 0x21, 0x12, + 0x12, 0xa3, 0x01, 0x21, 0x21, 0xa2, 0x10, 0x20, 0x20, 0x21, 0x21, 0x11, 0x12, 0x10, 0x11, 0xa2, + 0x00, 0xa0, 0x01, 0x10, 0x10, 0x21, 0x21, 0x12, 0x12, 0x81, 0x02, 0x22, 0x11, 0x13, 0x03, 0xa3, + 0x01, 0x10, 0x10, 0x21, 0x21, 0x03, 0x03, 0xa3, 0x01, 0x10, 0x10, 0x21, 0x21, 0x12, 0x12, 0x02, + 0x12, 0x23, 0x23, 0x83, 0x02, 0x05, 0x04, 0x14, 0x14, 0x23, 0x22, 0xa4, 0x14, 0x10, 0x10, 0x01, + 0x01, 0x12, 0x10, 0x20, 0x20, 0xa4, 0x14, 0x15, 0x15, 0x85, 0x20, 0xa1, 0x10, 0x20, 0x20, 0x21, + 0x21, 0x11, 0x11, 0x90, 0x01, 0x12, 0x12, 0x03, 0x11, 0x22, 0x22, 0x93, 0x00, 0x01, 0x02, 0x20, + 0x12, 0x13, 0x13, 0x23, 0x22, 0xa4, 0x00, 0x01, 0x02, 0x20, 0x12, 0x22, 0x22, 0x13, 0x13, 0x14, + 0x14, 0xa4, 0x00, 0x10, 0x10, 0x11, 0x11, 0x01, 0x11, 0x02, 0x02, 0x20, 0x12, 0x13, 0x13, 0x23, + 0x22, 0xa4, 0x10, 0x10, 0x11, 0x12, 0x12, 0x03, 0x03, 0x14, 0x14, 0xa3, 0x04, 0x02, 0x02, 0x11, + 0x11, 0x22, 0x22, 0x24, 0x03, 0x23, 0x10, 0xa1, 0x04, 0x02, 0x02, 0x11, 0x11, 0x22, 0x22, 0x24, + 0x03, 0x23, 0x01, 0x90, 0x04, 0x02, 0x02, 0x11, 0x11, 0x22, 0x22, 0x24, 0x03, 0x23, 0x01, 0x10, + 0x10, 0xa1, 0x04, 0x02, 0x02, 0x11, 0x11, 0x22, 0x22, 0x24, 0x03, 0x23, 0x01, 0x10, 0x10, 0x11, + 0x11, 0xa0, 0x04, 0x02, 0x02, 0x11, 0x11, 0x22, 0x22, 0x24, 0x03, 0x23, 0x00, 0x00, 0x20, 0xa0, + 0x04, 0x02, 0x02, 0x11, 0x11, 0x22, 0x22, 0x24, 0x03, 0x23, 0x00, 0x20, 0x20, 0x11, 0x11, 0x80, + 0x00, 0x04, 0x02, 0x22, 0x00, 0x11, 0x10, 0x14, 0x10, 0x20, 0x14, 0xa4, 0x23, 0x14, 0x14, 0x03, + 0x03, 0x01, 0x01, 0x10, 0x10, 0x21, 0x14, 0x15, 0x15, 0x85, 0x02, 0x22, 0x02, 0x04, 0x04, 0x24, + 0x03, 0x13, 0x00, 0x91, 0x02, 0x22, 0x02, 0x04, 0x04, 0x24, 0x03, 0x13, 0x11, 0xa0, 0x02, 0x22, + 0x02, 0x04, 0x04, 0x24, 0x03, 0x13, 0x01, 0x10, 0x10, 0xa1, 0x02, 0x22, 0x02, 0x04, 0x04, 0x24, + 0x03, 0x13, 0x00, 0x00, 0x20, 0xa0, 0x02, 0x22, 0x12, 0x14, 0x04, 0x24, 0x00, 0x91, 0x02, 0x22, + 0x12, 0x14, 0x04, 0x24, 0x11, 0xa0, 0x02, 0x22, 0x12, 0x14, 0x04, 0x24, 0x01, 0x10, 0x10, 0xa1, + 0x02, 0x22, 0x12, 0x14, 0x04, 0x24, 0x20, 0x20, 0x00, 0x80, 0x00, 0x10, 0x10, 0x21, 0x21, 0x23, + 0x23, 0x14, 0x14, 0x04, 0x04, 0x00, 0x02, 0x92, 0x04, 0x02, 0x02, 0x24, 0x24, 0x22, 0x01, 0x10, + 0x10, 0x11, 0x11, 0xa0, 0x02, 0x22, 0x22, 0x24, 0x24, 0x04, 0x04, 0x02, 0x00, 0x91, 0x02, 0x22, + 0x22, 0x24, 0x24, 0x04, 0x04, 0x02, 0x11, 0xa0, 0x02, 0x22, 0x22, 0x24, 0x24, 0x04, 0x04, 0x02, + 0x11, 0x20, 0x00, 0x91, 0x02, 0x22, 0x22, 0x24, 0x24, 0x04, 0x04, 0x02, 0x01, 0x10, 0x10, 0x11, + 0x11, 0xa0, 0x01, 0x21, 0x21, 0x24, 0x24, 0x04, 0x04, 0x01, 0x00, 0x00, 0x20, 0xa0, 0x01, 0x23, + 0x03, 0xa1, 0x01, 0x10, 0x10, 0x21, 0x21, 0x23, 0x23, 0x14, 0x14, 0x03, 0x03, 0x01, 0x03, 0xa1, + 0x01, 0x04, 0x04, 0x24, 0x24, 0x21, 0x11, 0xa0, 0x01, 0x04, 0x04, 0x24, 0x24, 0x21, 0x00, 0x91, + 0x02, 0x04, 0x04, 0x24, 0x24, 0x22, 0x01, 0x10, 0x10, 0xa1, 0x01, 0x04, 0x04, 0x24, 0x24, 0x21, + 0x00, 0x00, 0x20, 0xa0, 0x01, 0x02, 0x02, 0x13, 0x13, 0x14, 0x13, 0x22, 0x22, 0x21, 0x11, 0xa0, + 0x00, 0x04, 0x01, 0x11, 0x11, 0x22, 0x22, 0x13, 0x13, 0x83, 0x00, 0x05, 0x00, 0x10, 0x10, 0x21, + 0x21, 0x12, 0x02, 0x22, 0x22, 0x24, 0x24, 0x84, 0x02, 0x04, 0x04, 0x24, 0x24, 0x22, 0x23, 0x12, + 0x12, 0x02, 0x00, 0x91, 0x02, 0x04, 0x04, 0x24, 0x24, 0x22, 0x23, 0x12, 0x12, 0x02, 0x11, 0xa0, + 0x02, 0x04, 0x04, 0x24, 0x24, 0x22, 0x23, 0x12, 0x12, 0x02, 0x01, 0x10, 0x10, 0xa1, 0x02, 0x04, + 0x04, 0x24, 0x24, 0x22, 0x23, 0x12, 0x12, 0x02, 0x01, 0x10, 0x10, 0x11, 0x11, 0xa0, 0x02, 0x04, + 0x04, 0x24, 0x24, 0x22, 0x23, 0x12, 0x12, 0x02, 0x01, 0x01, 0x21, 0xa1, 0x02, 0x04, 0x04, 0x24, + 0x24, 0x22, 0x23, 0x12, 0x12, 0x02, 0x01, 0x10, 0x10, 0x21, 0x21, 0x81, 0x02, 0x13, 0x02, 0x04, + 0x04, 0x24, 0x12, 0x14, 0x12, 0x22, 0x13, 0x23, 0x22, 0xa3, 0x03, 0x04, 0x04, 0x24, 0x03, 0x12, + 0x12, 0x22, 0x14, 0x15, 0x15, 0x85, 0x24, 0x04, 0x04, 0x02, 0x02, 0x22, 0x22, 0x23, 0x23, 0x13, + 0x00, 0x91, 0x24, 0x04, 0x04, 0x02, 0x02, 0x22, 0x22, 0x23, 0x23, 0x13, 0x11, 0xa0, 0x24, 0x04, + 0x04, 0x02, 0x02, 0x22, 0x22, 0x23, 0x23, 0x13, 0x01, 0x10, 0x10, 0xa1, 0x24, 0x04, 0x04, 0x02, + 0x02, 0x22, 0x22, 0x23, 0x23, 0x13, 0x01, 0x01, 0x21, 0xa1, 0x12, 0x14, 0x00, 0x91, 0x12, 0x14, + 0x11, 0xa0, 0x12, 0x14, 0x01, 0x10, 0x10, 0xa1, 0x12, 0x14, 0x01, 0x01, 0x21, 0xa1, 0x00, 0x22, + 0x11, 0x20, 0x02, 0x22, 0x22, 0x24, 0x24, 0x04, 0x04, 0x82, 0x02, 0x04, 0x03, 0x12, 0x12, 0x23, + 0x23, 0x24, 0x01, 0x10, 0x10, 0x11, 0x11, 0xa0, 0x02, 0x04, 0x04, 0x24, 0x24, 0x22, 0x22, 0x02, + 0x00, 0x91, 0x02, 0x04, 0x04, 0x24, 0x24, 0x22, 0x22, 0x02, 0x11, 0xa0, 0x02, 0x04, 0x04, 0x24, + 0x24, 0x22, 0x22, 0x02, 0x01, 0x10, 0x10, 0xa1, 0x02, 0x04, 0x04, 0x24, 0x24, 0x22, 0x22, 0x02, + 0x01, 0x10, 0x10, 0x11, 0x11, 0xa0, 0x02, 0x04, 0x04, 0x24, 0x24, 0x22, 0x22, 0x02, 0x01, 0x01, + 0x21, 0xa1, 0x11, 0x11, 0x02, 0x22, 0x13, 0x93, 0x02, 0x04, 0x04, 0x24, 0x24, 0x22, 0x22, 0x02, + 0x04, 0xa2, 0x02, 0x04, 0x04, 0x14, 0x14, 0x23, 0x24, 0x22, 0x00, 0x91, 0x02, 0x04, 0x04, 0x14, + 0x14, 0x23, 0x24, 0x22, 0x11, 0xa0, 0x02, 0x04, 0x04, 0x14, 0x14, 0x23, 0x24, 0x22, 0x01, 0x10, + 0x10, 0xa1, 0x02, 0x04, 0x04, 0x14, 0x14, 0x23, 0x24, 0x22, 0x01, 0x01, 0x21, 0xa1, 0x02, 0x03, + 0x03, 0x14, 0x22, 0x23, 0x23, 0x05, 0x11, 0xa0, 0x00, 0x04, 0x02, 0x11, 0x11, 0x22, 0x22, 0x13, + 0x13, 0x82, 0x02, 0x03, 0x03, 0x14, 0x22, 0x23, 0x23, 0x05, 0x01, 0x01, 0x21, 0xa1, }; + + + +void CNFGDrawText( const char * text, int scale ) +{ + const unsigned char * lmap; + float iox = (float)CNFGPenX; + float ioy = (float)CNFGPenY; + + int place = 0; + unsigned short index; + int bQuit = 0; + while( text[place] ) + { + unsigned char c = text[place]; + + switch( c ) + { + case 9: + iox += 12 * scale; + break; + case 10: + iox = (float)CNFGPenX; + ioy += 6 * scale; + break; + default: + index = FontCharMap[c]; + if( index == 65535 ) + { + iox += 3 * scale; + break; + } + + lmap = &FontCharData[index]; + do + { + int x1 = (int)((((*lmap) & 0x70)>>4)*scale + iox); + int y1 = (int)(((*lmap) & 0x0f)*scale + ioy); + int x2 = (int)((((*(lmap+1)) & 0x70)>>4)*scale + iox); + int y2 = (int)(((*(lmap+1)) & 0x0f)*scale + ioy); + lmap++; + CNFGTackSegment( x1, y1, x2, y2 ); + bQuit = *lmap & 0x80; + lmap++; + } while( !bQuit ); + + iox += 3 * scale; + } + place++; + } +} + + +void CNFGDrawBox( int x1, int y1, int x2, int y2 ) +{ + unsigned lc = CNFGLastColor; + CNFGColor( CNFGDialogColor ); + CNFGTackRectangle( x1, y1, x2, y2 ); + CNFGColor( lc ); + CNFGTackSegment( x1, y1, x2, y1 ); + CNFGTackSegment( x2, y1, x2, y2 ); + CNFGTackSegment( x2, y2, x1, y2 ); + CNFGTackSegment( x1, y2, x1, y1 ); +} + +void CNFGGetTextExtents( const char * text, int * w, int * h, int textsize ) +{ + int charsx = 0; + int charsy = 1; + int charsline = 0; + const char * s; + + for( s = text; *s; s++ ) + { + if( *s == '\n' ) + { + charsline = 0; + if( *(s+1) ) + charsy++; + } + else + { + charsline++; + if( charsline > charsx ) + charsx = charsline; + } + } + + *w = charsx * textsize * 3 + textsize; + *h = charsy * textsize * 6; +} + +void CNFGDrawTextbox( int x, int y, const char * text, int textsize ) +{ + int w; + int h; + + CNFGGetTextExtents( text, &w, &h, textsize ); + + CNFGDrawBox( x, y, x + w, y + h ); + CNFGPenX = x + textsize; + CNFGPenY = y + textsize; + CNFGDrawText( text, textsize ); +} diff --git a/redist/CNFGFunctions.h b/redist/CNFGFunctions.h new file mode 100644 index 0000000..9ecb1bd --- /dev/null +++ b/redist/CNFGFunctions.h @@ -0,0 +1,59 @@ +//Copyright (c) 2011 <>< Charles Lohr - Under the MIT/x11 or NewBSD License you choose. + +#ifndef _DRAWFUCNTIONS_H +#define _DRAWFUCNTIONS_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +typedef struct { + short x, y; +} RDPoint; + +extern int CNFGPenX, CNFGPenY; +extern uint32_t CNFGBGColor; +extern uint32_t CNFGLastColor; +extern uint32_t CNFGDialogColor; //background for boxes + +void CNFGDrawText( const char * text, int scale ); +void CNFGDrawBox( int x1, int y1, int x2, int y2 ); +void CNFGGetTextExtents( const char * text, int * w, int * h, int textsize ); +void CNFGDrawTextbox( int x, int y, const char * text, int textsize ); //ignores pen. + +//To be provided by driver. +uint32_t CNFGColor( uint32_t RGB ); +void CNFGUpdateScreenWithBitmap( unsigned long * data, int w, int h ); +void CNFGTackPixel( short x1, short y1 ); +void CNFGTackSegment( short x1, short y1, short x2, short y2 ); +void CNFGTackRectangle( short x1, short y1, short x2, short y2 ); +void CNFGTackPoly( RDPoint * points, int verts ); +void CNFGClearFrame(); +void CNFGSwapBuffers(); + +void CNFGGetDimensions( short * x, short * y ); +void CNFGSetup( const char * WindowName, int w, int h ); +void CNFGSetupFullscreen( const char * WindowName, int screen_number ); +void CNFGHandleInput(); + + +//You must provide: +void HandleKey( int keycode, int bDown ); +void HandleButton( int x, int y, int button, int bDown ); +void HandleMotion( int x, int y, int mask ); +void HandleDestroy(); + + +//Internal function for resizing rasterizer for rasterizer-mode. +void CNFGInternalResize( short x, short y ); //don't call this. + + +#ifdef __cplusplus +}; +#endif + + +#endif + diff --git a/redist/CNFGNullDriver.c b/redist/CNFGNullDriver.c new file mode 100644 index 0000000..34346cc --- /dev/null +++ b/redist/CNFGNullDriver.c @@ -0,0 +1,74 @@ +//Copyright (c) 2017 <>< Charles Lohr - Under the MIT/x11 or NewBSD License you choose. + +#include "DrawFunctions.h" + +static int w, h; +void CNFGGetDimensions( short * x, short * y ) +{ + *x = w; + *y = h; +} + +static void InternalLinkScreenAndGo( const char * WindowName ) +{ +} + +void CNFGSetupFullscreen( const char * WindowName, int screen_no ) +{ + CNFGSetup( WindowName, 640, 480 ); +} + + +void CNFGTearDown() +{ +} + +void CNFGSetup( const char * WindowName, int sw, int sh ) +{ + w = sw; + h = sh; +} + +void CNFGHandleInput() +{ +} + + +void CNFGUpdateScreenWithBitmap( unsigned long * data, int w, int h ) +{ +} + + +#ifndef RASTERIZER + + +uint32_t CNFGColor( uint32_t RGB ) +{ +} + +void CNFGClearFrame() +{ +} + +void CNFGSwapBuffers() +{ +} + +void CNFGTackSegment( short x1, short y1, short x2, short y2 ) +{ +} + +void CNFGTackPixel( short x1, short y1 ) +{ +} + +void CNFGTackRectangle( short x1, short y1, short x2, short y2 ) +{ +} + +void CNFGTackPoly( RDPoint * points, int verts ) +{ +} + +#endif + diff --git a/redist/CNFGRasterizer.h b/redist/CNFGRasterizer.h new file mode 100644 index 0000000..1b8e2dd --- /dev/null +++ b/redist/CNFGRasterizer.h @@ -0,0 +1,247 @@ +//Don't call this file yourself. It is intended to be included in any drivers which want to support the rasterizer plugin. + +#ifdef RASTERIZER +#include "CNFGFunctions.h" +#include +#include + +static uint32_t * buffer = 0; +static short bufferx; +static short buffery; + + +void CNFGInternalResize( short x, short y ) +{ + bufferx = x; + buffery = y; + if( buffer ) free( buffer ); + buffer = malloc( bufferx * buffery * 4 ); +} + +static uint32_t SWAPS( uint32_t r ) +{ + uint32_t ret = (r&0xFF)<<16; + r>>=8; + ret |= (r&0xff)<<8; + r>>=8; + ret |= r; + return ret; +} + +uint32_t CNFGColor( uint32_t RGB ) +{ + CNFGLastColor = SWAPS(RGB); + return CNFGLastColor; +} + +void CNFGTackSegment( short x1, short y1, short x2, short y2 ) +{ + short tx, ty; + float slope, lp; + + short dx = x2 - x1; + short dy = y2 - y1; + + if( !buffer ) return; + + if( dx < 0 ) dx = -dx; + if( dy < 0 ) dy = -dy; + + if( dx > dy ) + { + short minx = (x1 < x2)?x1:x2; + short maxx = (x1 < x2)?x2:x1; + short miny = (x1 < x2)?y1:y2; + short maxy = (x1 < x2)?y2:y1; + float thisy = miny; + slope = (float)(maxy-miny) / (float)(maxx-minx); + + for( tx = minx; tx <= maxx; tx++ ) + { + ty = thisy; + if( tx < 0 || ty < 0 || ty >= buffery ) continue; + if( tx >= bufferx ) break; + buffer[ty * bufferx + tx] = CNFGLastColor; + thisy += slope; + } + } + else + { + short minx = (y1 < y2)?x1:x2; + short maxx = (y1 < y2)?x2:x1; + short miny = (y1 < y2)?y1:y2; + short maxy = (y1 < y2)?y2:y1; + float thisx = minx; + slope = (float)(maxx-minx) / (float)(maxy-miny); + + for( ty = miny; ty <= maxy; ty++ ) + { + tx = thisx; + if( ty < 0 || tx < 0 || tx >= bufferx ) continue; + if( ty >= buffery ) break; + buffer[ty * bufferx + tx] = CNFGLastColor; + thisx += slope; + } + } +} +void CNFGTackRectangle( short x1, short y1, short x2, short y2 ) +{ + short minx = (x1=x2)?x1:x2; + short maxy = (y1>=y2)?y1:y2; + + short x, y; + + if( minx < 0 ) minx = 0; + if( miny < 0 ) miny = 0; + if( maxx >= bufferx ) maxx = bufferx-1; + if( maxy >= buffery ) maxy = buffery-1; + + for( y = miny; y <= maxy; y++ ) + { + uint32_t * bufferstart = &buffer[y * bufferx + minx]; + for( x = minx; x <= maxx; x++ ) + { + (*bufferstart++) = CNFGLastColor; + } + } +} + +void CNFGTackPoly( RDPoint * points, int verts ) +{ + short minx = 10000, miny = 10000; + short maxx =-10000, maxy =-10000; + short i, x, y; + + //Just in case... + if( verts > 32767 ) return; + + for( i = 0; i < verts; i++ ) + { + RDPoint * p = &points[i]; + if( p->x < minx ) minx = p->x; + if( p->y < miny ) miny = p->y; + if( p->x > maxx ) maxx = p->x; + if( p->y > maxy ) maxy = p->y; + } + + if( miny < 0 ) miny = 0; + if( maxy >= buffery ) maxy = buffery-1; + + for( y = miny; y <= maxy; y++ ) + { + short startfillx = maxx; + short endfillx = minx; + + //Figure out what line segments intersect this line. + for( i = 0; i < verts; i++ ) + { + short pl = i + 1; + if( pl == verts ) pl = 0; + + RDPoint ptop; + RDPoint pbot; + + ptop.x = points[i].x; + ptop.y = points[i].y; + pbot.x = points[pl].x; + pbot.y = points[pl].y; +//printf( "Poly: %d %d\n", pbot.y, ptop.y ); + + if( pbot.y < ptop.y ) + { + RDPoint ptmp; + ptmp.x = pbot.x; + ptmp.y = pbot.y; + pbot.x = ptop.x; + pbot.y = ptop.y; + ptop.x = ptmp.x; + ptop.y = ptmp.y; + } + + //Make sure this line segment is within our range. +//printf( "PT: %d %d %d\n", y, ptop.y, pbot.y ); + if( ptop.y <= y && pbot.y >= y ) + { + short diffy = pbot.y - ptop.y; + uint32_t placey = (uint32_t)(y - ptop.y)<<16; //Scale by 16 so we can do integer math. + short diffx = pbot.x - ptop.x; + short isectx; + + if( diffy == 0 ) + { + if( pbot.x < ptop.x ) + { + if( startfillx > pbot.x ) startfillx = pbot.x; + if( endfillx < ptop.x ) endfillx = ptop.x; + } + else + { + if( startfillx > ptop.x ) startfillx = ptop.x; + if( endfillx < pbot.x ) endfillx = pbot.x; + } + } + else + { + //Inner part is scaled by 65536, outer part must be scaled back. + isectx = (( (placey / diffy) * diffx + 32768 )>>16) + ptop.x; + if( isectx < startfillx ) startfillx = isectx; + if( isectx > endfillx ) endfillx = isectx; + } +//printf( "R: %d %d %d\n", pbot.x, ptop.x, isectx ); + } + } + +//printf( "%d %d %d\n", y, startfillx, endfillx ); + + if( endfillx >= bufferx ) endfillx = bufferx - 1; + if( endfillx >= bufferx ) endfillx = buffery - 1; + if( startfillx < 0 ) startfillx = 0; + if( startfillx < 0 ) startfillx = 0; + + unsigned int * bufferstart = &buffer[y * bufferx + startfillx]; + for( x = startfillx; x <= endfillx; x++ ) + { + (*bufferstart++) = CNFGLastColor; + } + } +//exit(1); +} + + +void CNFGClearFrame() +{ + int i, m; + uint32_t col = 0; + short x, y; + CNFGGetDimensions( &x, &y ); + if( x != bufferx || y != buffery || !buffer ) + { + bufferx = x; + buffery = y; + buffer = malloc( x * y * 8 ); + } + + m = x * y; + col = CNFGColor( CNFGBGColor ); + for( i = 0; i < m; i++ ) + { +//printf( "Got: %d %p %d\n", m, buffer, i ); + buffer[i] = col; + } +} + +void CNFGTackPixel( short x, short y ) +{ + if( x < 0 || y < 0 || x >= bufferx || y >= buffery ) return; + buffer[x+bufferx*y] = CNFGLastColor; +} + +void CNFGSwapBuffers() +{ + CNFGUpdateScreenWithBitmap( (long unsigned int*)buffer, bufferx, buffery ); +} + + +#endif diff --git a/redist/CNFGWinDriver.c b/redist/CNFGWinDriver.c new file mode 100644 index 0000000..a029419 --- /dev/null +++ b/redist/CNFGWinDriver.c @@ -0,0 +1,289 @@ +//Copyright (c) 2011 <>< Charles Lohr - Under the MIT/x11 or NewBSD License you choose. +//Portion from: http://en.wikibooks.org/wiki/Windows_Programming/Window_Creation + + +#include "CNFGFunctions.h" +#include +#include +#include //for alloca + +static HBITMAP lsBitmap; +static HINSTANCE lhInstance; +static HWND lsHWND; +static HDC lsWindowHDC; +static HDC lsHDC; + + +#ifdef RASTERIZER +#include "CNFGRasterizer.h" + +void InternalHandleResize() +{ + if( lsBitmap ) DeleteObject( lsBitmap ); + + CNFGInternalResize( bufferx, buffery ); + lsBitmap = CreateBitmap( bufferx, buffery, 1, 32, buffer ); + SelectObject( lsHDC, lsBitmap ); +} +#else +static int bufferx, buffery; +static int bufferx, buffery; +static void InternalHandleResize(); +#endif + + +void CNFGGetDimensions( short * x, short * y ) +{ + *x = bufferx; + *y = buffery; +} + + + +void CNFGUpdateScreenWithBitmap( unsigned long * data, int w, int h ) +{ + RECT r; + + int a = SetBitmapBits(lsBitmap,w*h*4,data); + a = BitBlt(lsWindowHDC, 0, 0, w, h, lsHDC, 0, 0, SRCCOPY); + UpdateWindow( lsHWND ); + + int thisw, thish; + + //Check to see if the window is closed. + if( !IsWindow( lsHWND ) ) + { + exit( 0 ); + } + + GetClientRect( lsHWND, &r ); + thisw = r.right - r.left; + thish = r.bottom - r.top; + if( thisw != bufferx || thish != buffery ) + { + bufferx = thisw; + buffery = thish; + InternalHandleResize(); + } +} + + +void CNFGTearDown() +{ + PostQuitMessage(0); +} + +//This was from the article +LRESULT CALLBACK MyWndProc(HWND hwnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_DESTROY: + HandleDestroy(); + CNFGTearDown(); + return 0; + } + return DefWindowProc(hwnd, msg, wParam, lParam); +} + +//This was from the article, too... well, mostly. +void CNFGSetup( const char * name_of_window, int width, int height ) +{ + static LPSTR szClassName = "MyClass"; + RECT client, window; + WNDCLASS wnd; + int w, h, wd, hd; + HINSTANCE hInstance = GetModuleHandle(NULL); + + bufferx = width; + buffery = height; + + wnd.style = CS_HREDRAW | CS_VREDRAW; //we will explain this later + wnd.lpfnWndProc = MyWndProc; + wnd.cbClsExtra = 0; + wnd.cbWndExtra = 0; + wnd.hInstance = hInstance; + wnd.hIcon = LoadIcon(NULL, IDI_APPLICATION); //default icon + wnd.hCursor = LoadCursor(NULL, IDC_ARROW); //default arrow mouse cursor + wnd.hbrBackground = (HBRUSH)(COLOR_BACKGROUND); + wnd.lpszMenuName = NULL; //no menu + wnd.lpszClassName = szClassName; + + if(!RegisterClass(&wnd)) //register the WNDCLASS + { + MessageBox(NULL, "This Program Requires Windows NT", "Error", MB_OK); + } + + lsHWND = CreateWindow(szClassName, + name_of_window, //name_of_window, + WS_OVERLAPPEDWINDOW, //basic window style + CW_USEDEFAULT, + CW_USEDEFAULT, //set starting point to default value + bufferx, + buffery, //set all the dimensions to default value + NULL, //no parent window + NULL, //no menu + hInstance, + NULL); //no parameters to pass + + + lsWindowHDC = GetDC( lsHWND ); + + lsHDC = CreateCompatibleDC( lsWindowHDC ); + lsBitmap = CreateCompatibleBitmap( lsWindowHDC, bufferx, buffery ); + SelectObject( lsHDC, lsBitmap ); + + //lsClearBrush = CreateSolidBrush( CNFGBGColor ); + //lsHBR = CreateSolidBrush( 0xFFFFFF ); + //lsHPEN = CreatePen( PS_SOLID, 0, 0xFFFFFF ); + + ShowWindow(lsHWND, 1); //display the window on the screen + + //Once set up... we have to change the window's borders so we get the client size right. + GetClientRect( lsHWND, &client ); + GetWindowRect( lsHWND, &window ); + w = ( window.right - window.left); + h = ( window.bottom - window.top); + wd = w - client.right; + hd = h - client.bottom; + MoveWindow( lsHWND, window.left, window.top, bufferx + wd, buffery + hd, 1 ); + + InternalHandleResize(); +} + +void CNFGHandleInput() +{ + int ldown = 0; + + MSG msg; + while( PeekMessage( &msg, lsHWND, 0, 0xFFFF, 1 ) ) + { + TranslateMessage(&msg); + + switch( msg.message ) + { + case WM_MOUSEMOVE: + HandleMotion( (msg.lParam & 0xFFFF), (msg.lParam>>16) & 0xFFFF, ( (msg.wParam & 0x01)?1:0) | ((msg.wParam & 0x02)?2:0) | ((msg.wParam & 0x10)?4:0) ); + break; + case WM_LBUTTONDOWN: HandleButton( (msg.lParam & 0xFFFF), (msg.lParam>>16) & 0xFFFF, 1, 1 ); break; + case WM_RBUTTONDOWN: HandleButton( (msg.lParam & 0xFFFF), (msg.lParam>>16) & 0xFFFF, 2, 1 ); break; + case WM_MBUTTONDOWN: HandleButton( (msg.lParam & 0xFFFF), (msg.lParam>>16) & 0xFFFF, 3, 1 ); break; + case WM_LBUTTONUP: HandleButton( (msg.lParam & 0xFFFF), (msg.lParam>>16) & 0xFFFF, 1, 0 ); break; + case WM_RBUTTONUP: HandleButton( (msg.lParam & 0xFFFF), (msg.lParam>>16) & 0xFFFF, 2, 0 ); break; + case WM_MBUTTONUP: HandleButton( (msg.lParam & 0xFFFF), (msg.lParam>>16) & 0xFFFF, 3, 0 ); break; + case WM_KEYDOWN: + case WM_KEYUP: + HandleKey( tolower( msg.wParam ), (msg.message==WM_KEYDOWN) ); + break; + default: + DispatchMessage(&msg); + break; + } + } +} + + +#ifndef RASTERIZER + +static HBITMAP lsBackBitmap; +static HDC lsWindowHDC; +static HBRUSH lsHBR; +static HPEN lsHPEN; +static HBRUSH lsClearBrush; + +static void InternalHandleResize() +{ + DeleteObject( lsBackBitmap ); + lsBackBitmap = CreateCompatibleBitmap( lsHDC, bufferx, buffery ); + SelectObject( lsHDC, lsBackBitmap ); +} + +uint32_t CNFGColor( uint32_t RGB ) +{ + CNFGLastColor = RGB; + + DeleteObject( lsHBR ); + lsHBR = CreateSolidBrush( RGB ); + SelectObject( lsHDC, lsHBR ); + + DeleteObject( lsHPEN ); + lsHPEN = CreatePen( PS_SOLID, 0, RGB ); + SelectObject( lsHDC, lsHPEN ); + + return RGB; +} + +void CNFGTackSegment( short x1, short y1, short x2, short y2 ) +{ + POINT pt[2] = { {x1, y1}, {x2, y2} }; + Polyline( lsHDC, pt, 2 ); + SetPixel( lsHDC, x1, y1, CNFGLastColor ); + SetPixel( lsHDC, x2, y2, CNFGLastColor ); +} + +void CNFGTackRectangle( short x1, short y1, short x2, short y2 ) +{ + RECT r; + if( x1 < x2 ) { r.left = x1; r.right = x2; } + else { r.left = x2; r.right = x1; } + if( y1 < y2 ) { r.top = y1; r.bottom = y2; } + else { r.top = y2; r.bottom = y1; } + FillRect( lsHDC, &r, lsHBR ); +} + +void CNFGClearFrame() +{ + RECT r = { 0, 0, bufferx, buffery }; + DeleteObject( lsClearBrush ); + lsClearBrush = CreateSolidBrush( CNFGBGColor ); + SelectObject( lsHDC, lsClearBrush ); + + FillRect( lsHDC, &r, lsClearBrush ); +} + +void CNFGTackPoly( RDPoint * points, int verts ) +{ + int i; + POINT * t = (POINT*)alloca( sizeof( POINT ) * verts ); + for( i = 0; i < verts; i++ ) + { + t[i].x = points[i].x; + t[i].y = points[i].y; + } + Polygon( lsHDC, t, verts ); +} + + +void CNFGTackPixel( short x1, short y1 ) +{ + SetPixel( lsHDC, x1, y1, CNFGLastColor ); +} + +void CNFGSwapBuffers() +{ + int thisw, thish; + + RECT r; + BitBlt( lsWindowHDC, 0, 0, bufferx, buffery, lsHDC, 0, 0, SRCCOPY ); + UpdateWindow( lsHWND ); + //Check to see if the window is closed. + if( !IsWindow( lsHWND ) ) + { + exit( 0 ); + } + + GetClientRect( lsHWND, &r ); + thisw = r.right - r.left; + thish = r.bottom - r.top; + + if( thisw != bufferx || thish != buffery ) + { + bufferx = thisw; + buffery = thish; + InternalHandleResize(); + } +} + +void CNFGInternalResize( short bufferx, short buffery ) { } +#endif + diff --git a/redist/CNFGXDriver.c b/redist/CNFGXDriver.c new file mode 100644 index 0000000..8a8904a --- /dev/null +++ b/redist/CNFGXDriver.c @@ -0,0 +1,304 @@ +//Copyright (c) 2011 <>< Charles Lohr - Under the MIT/x11 or NewBSD License you choose. +//portions from +//http://www.xmission.com/~georgeps/documentation/tutorials/Xlib_Beginner.html + +//#define HAS_XINERAMA + +#include "CNFGFunctions.h" + +#include +#include +#include +#include +#include +#ifdef HAS_XINERAMA +#include +#include +#endif +#include +#include + +XWindowAttributes CNFGWinAtt; +XClassHint *CNFGClassHint; +Display *CNFGDisplay; +Window CNFGWindow; +Pixmap CNFGPixmap; +GC CNFGGC; +GC CNFGWindowGC; +int FullScreen = 0; + +void CNFGGetDimensions( short * x, short * y ) +{ + static int lastx; + static int lasty; + + *x = CNFGWinAtt.width; + *y = CNFGWinAtt.height; + + if( lastx != *x || lasty != *y ) + { + lastx = *x; + lasty = *y; + CNFGInternalResize( lastx, lasty ); + } +} + +static void InternalLinkScreenAndGo( const char * WindowName ) +{ + XGetWindowAttributes( CNFGDisplay, CNFGWindow, &CNFGWinAtt ); + + XGetClassHint( CNFGDisplay, CNFGWindow, CNFGClassHint ); + if (!CNFGClassHint) { + CNFGClassHint = XAllocClassHint(); + if (CNFGClassHint) { + CNFGClassHint->res_name = "cnping"; + CNFGClassHint->res_class = "cnping"; + XSetClassHint( CNFGDisplay, CNFGWindow, CNFGClassHint ); + } else { + fprintf( stderr, "Failed to allocate XClassHint!\n" ); + } + } else { + fprintf( stderr, "Pre-existing XClassHint\n" ); + } + + XSelectInput (CNFGDisplay, CNFGWindow, KeyPressMask | KeyReleaseMask | ButtonPressMask | ButtonReleaseMask | ExposureMask | PointerMotionMask ); + XSetStandardProperties( CNFGDisplay, CNFGWindow, WindowName, WindowName, None, NULL, 0, NULL ); + + CNFGWindowGC = XCreateGC(CNFGDisplay, CNFGWindow, 0, 0); + + CNFGPixmap = XCreatePixmap( CNFGDisplay, CNFGWindow, CNFGWinAtt.width, CNFGWinAtt.height, CNFGWinAtt.depth ); + CNFGGC = XCreateGC(CNFGDisplay, CNFGPixmap, 0, 0); +} + +void CNFGSetupFullscreen( const char * WindowName, int screen_no ) +{ +#ifdef HAS_XINERAMA + XineramaScreenInfo *screeninfo = NULL; + int screens; + int event_basep, error_basep, a, b; + CNFGDisplay = XOpenDisplay(NULL); + int screen = XDefaultScreen(CNFGDisplay); + int xpos, ypos; + + if (!XShapeQueryExtension(CNFGDisplay, &event_basep, &error_basep)) + { + fprintf( stderr, "X-Server does not support shape extension" ); + exit( 1 ); + } + + Visual * visual = DefaultVisual(CNFGDisplay, screen); + CNFGWinAtt.depth = DefaultDepth(CNFGDisplay, screen); + + if (XineramaQueryExtension(CNFGDisplay, &a, &b ) && + (screeninfo = XineramaQueryScreens(CNFGDisplay, &screens)) && + XineramaIsActive(CNFGDisplay) && screen_no >= 0 && + screen_no < screens ) { + + CNFGWinAtt.width = screeninfo[screen_no].width; + CNFGWinAtt.height = screeninfo[screen_no].height; + xpos = screeninfo[screen_no].x_org; + ypos = screeninfo[screen_no].y_org; + } else + { + CNFGWinAtt.width = XDisplayWidth(CNFGDisplay, screen); + CNFGWinAtt.height = XDisplayHeight(CNFGDisplay, screen); + xpos = 0; + ypos = 0; + } + if (screeninfo) + XFree(screeninfo); + + + XSetWindowAttributes setwinattr; + setwinattr.override_redirect = 1; + setwinattr.save_under = 1; + setwinattr.event_mask = StructureNotifyMask | SubstructureNotifyMask | ExposureMask | ButtonPressMask | ButtonReleaseMask | ButtonPressMask | PointerMotionMask | ButtonMotionMask | EnterWindowMask | LeaveWindowMask |KeyPressMask |KeyReleaseMask | SubstructureNotifyMask | FocusChangeMask; + setwinattr.border_pixel = 0; + + CNFGWindow = XCreateWindow(CNFGDisplay, XRootWindow(CNFGDisplay, screen), + xpos, ypos, CNFGWinAtt.width, CNFGWinAtt.height, + 0, CNFGWinAtt.depth, InputOutput, visual, CWBorderPixel | CWEventMask | CWOverrideRedirect | CWSaveUnder, &setwinattr); + + XMapWindow(CNFGDisplay, CNFGWindow); + XSetInputFocus( CNFGDisplay, CNFGWindow, RevertToParent, CurrentTime ); + XFlush(CNFGDisplay); + FullScreen = 1; +//printf( "%d %d %d %d\n", xpos, ypos, CNFGWinAtt.width, CNFGWinAtt.height ); + InternalLinkScreenAndGo( WindowName ); +/* + setwinattr.override_redirect = 1; + XChangeWindowAttributes( + CNFGDisplay, CNFGWindow, + CWBorderPixel | CWEventMask | CWOverrideRedirect, &setwinattr); +*/ +#else + CNFGSetup( WindowName, 640, 480 ); +#endif +} + + +void CNFGTearDown() +{ + if ( CNFGClassHint ) XFree( CNFGClassHint ); + if ( CNFGGC ) XFreeGC( CNFGDisplay, CNFGGC ); + if ( CNFGWindowGC ) XFreeGC( CNFGDisplay, CNFGWindowGC ); + if ( CNFGDisplay ) XCloseDisplay( CNFGDisplay ); + CNFGDisplay = NULL; + CNFGWindowGC = CNFGGC = NULL; + CNFGClassHint = NULL; +} + +void CNFGSetup( const char * WindowName, int w, int h ) +{ + CNFGDisplay = XOpenDisplay(NULL); + atexit( CNFGTearDown ); + XGetWindowAttributes( CNFGDisplay, RootWindow(CNFGDisplay, 0), &CNFGWinAtt ); + + int depth = CNFGWinAtt.depth; + CNFGWindow = XCreateWindow(CNFGDisplay, RootWindow(CNFGDisplay, 0), 1, 1, w, h, 0, depth, InputOutput, CopyFromParent, 0, 0 ); + XMapWindow(CNFGDisplay, CNFGWindow); + XFlush(CNFGDisplay); + + InternalLinkScreenAndGo( WindowName ); + + Atom WM_DELETE_WINDOW = XInternAtom( CNFGDisplay, "WM_DELETE_WINDOW", False ); + XSetWMProtocols( CNFGDisplay, CNFGWindow, &WM_DELETE_WINDOW, 1 ); + XSelectInput( CNFGDisplay, CNFGWindow, KeyPressMask | KeyReleaseMask | ButtonPressMask | ButtonReleaseMask | ExposureMask | PointerMotionMask ); +} + +void CNFGHandleInput() +{ + static int ButtonsDown; + XEvent report; + + int bKeyDirection = 1; + int r; + while( XPending( CNFGDisplay ) ) + { + r=XNextEvent( CNFGDisplay, &report ); + + bKeyDirection = 1; + switch (report.type) + { + case NoExpose: + break; + case Expose: + XGetWindowAttributes( CNFGDisplay, CNFGWindow, &CNFGWinAtt ); + if( CNFGPixmap ) XFreePixmap( CNFGDisplay, CNFGPixmap ); + CNFGPixmap = XCreatePixmap( CNFGDisplay, CNFGWindow, CNFGWinAtt.width, CNFGWinAtt.height, CNFGWinAtt.depth ); + if( CNFGGC ) XFreeGC( CNFGDisplay, CNFGGC ); + CNFGGC = XCreateGC(CNFGDisplay, CNFGPixmap, 0, 0); + break; + case KeyRelease: + bKeyDirection = 0; + case KeyPress: + HandleKey( XLookupKeysym(&report.xkey, 0), bKeyDirection ); + break; + case ButtonRelease: + bKeyDirection = 0; + case ButtonPress: + HandleButton( report.xbutton.x, report.xbutton.y, report.xbutton.button, bKeyDirection ); + ButtonsDown = (ButtonsDown & (~(1<>1 ); + break; + case ClientMessage: + // Only subscribed to WM_DELETE_WINDOW, so just exit + exit( 0 ); + break; + default: + printf( "Event: %d\n", report.type ); + } + } +} + + +void CNFGUpdateScreenWithBitmap( unsigned long * data, int w, int h ) +{ + static XImage *xi; + static int depth; + static int lw, lh; + static unsigned char * lbuffer; + int r, ls; + + if( !xi ) + { + int screen = DefaultScreen(CNFGDisplay); + Visual * visual = DefaultVisual(CNFGDisplay, screen); + depth = DefaultDepth(CNFGDisplay, screen)/8; +// xi = XCreateImage(CNFGDisplay, DefaultVisual( CNFGDisplay, DefaultScreen(CNFGDisplay) ), depth*8, ZPixmap, 0, (char*)data, w, h, 32, w*4 ); +// lw = w; +// lh = h; + } + + if( lw != w || lh != h ) + { + if( xi ) free( xi ); + xi = XCreateImage(CNFGDisplay, DefaultVisual( CNFGDisplay, DefaultScreen(CNFGDisplay) ), depth*8, ZPixmap, 0, (char*)data, w, h, 32, w*4 ); + lw = w; + lh = h; + } + + ls = lw * lh; + + XPutImage(CNFGDisplay, CNFGWindow, CNFGWindowGC, xi, 0, 0, 0, 0, w, h ); +} + + +#ifndef RASTERIZER + + +uint32_t CNFGColor( uint32_t RGB ) +{ + unsigned char red = RGB & 0xFF; + unsigned char grn = ( RGB >> 8 ) & 0xFF; + unsigned char blu = ( RGB >> 16 ) & 0xFF; + CNFGLastColor = RGB; + unsigned long color = (red<<16)|(grn<<8)|(blu); + XSetForeground(CNFGDisplay, CNFGGC, color); + return color; +} + +void CNFGClearFrame() +{ + XGetWindowAttributes( CNFGDisplay, CNFGWindow, &CNFGWinAtt ); + XSetForeground(CNFGDisplay, CNFGGC, CNFGColor(CNFGBGColor) ); + XFillRectangle(CNFGDisplay, CNFGPixmap, CNFGGC, 0, 0, CNFGWinAtt.width, CNFGWinAtt.height ); +} + +void CNFGSwapBuffers() +{ + XCopyArea(CNFGDisplay, CNFGPixmap, CNFGWindow, CNFGWindowGC, 0,0,CNFGWinAtt.width,CNFGWinAtt.height,0,0); + XFlush(CNFGDisplay); + if( FullScreen ) + XSetInputFocus( CNFGDisplay, CNFGWindow, RevertToParent, CurrentTime ); +} + +void CNFGTackSegment( short x1, short y1, short x2, short y2 ) +{ + XDrawLine( CNFGDisplay, CNFGPixmap, CNFGGC, x1, y1, x2, y2 ); +} + +void CNFGTackPixel( short x1, short y1 ) +{ + XDrawPoint( CNFGDisplay, CNFGPixmap, CNFGGC, x1, y1 ); +} + +void CNFGTackRectangle( short x1, short y1, short x2, short y2 ) +{ + XFillRectangle(CNFGDisplay, CNFGPixmap, CNFGGC, x1, y1, x2-x1, y2-y1 ); +} + +void CNFGTackPoly( RDPoint * points, int verts ) +{ + XFillPolygon(CNFGDisplay, CNFGPixmap, CNFGGC, (XPoint *)points, 3, Convex, CoordModeOrigin ); +} + +void CNFGInternalResize( short x, short y ) { } + +#else +#include "CNFGRasterizer.h" +#endif + diff --git a/redist/DrawFunctions.c b/redist/DrawFunctions.c deleted file mode 100644 index f4f27d2..0000000 --- a/redist/DrawFunctions.c +++ /dev/null @@ -1,277 +0,0 @@ -/* -Copyright (c) 2010, 2011 Charles Lohr - -Permission is hereby granted, free of charge, to any person obtaining -a copy of this software and associated documentation files (the -"Software"), to deal in the Software without restriction, including -without limitation the rights to use, copy, modify, merge, publish, -distribute, sublicense, and/or sell copies of the Software, and to -permit persons to whom the Software is furnished to do so, subject to -the following conditions: - -The above copyright notice and this permission notice shall be included -in all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS -OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF -MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. -IN NO EVENT SHALL THE X CONSORTIUM BE LIABLE FOR ANY CLAIM, DAMAGES OR -OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, -ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR -OTHER DEALINGS IN THE SOFTWARE. -*/ - -#include "DrawFunctions.h" -#include - -int CNFGPenX, CNFGPenY; -uint32_t CNFGBGColor; -uint32_t CNFGLastColor; -uint32_t CNFGDialogColor; //background for boxes - -const unsigned short FontCharMap[256] = { - 65535, 0, 10, 20, 32, 44, 56, 68, 70, 65535, 65535, 80, 92, 65535, 104, 114, - 126, 132, 138, 148, 156, 166, 180, 188, 200, 206, 212, 218, 224, 228, 238, 244, - 65535, 250, 254, 258, 266, 278, 288, 302, 304, 310, 316, 324, 328, 226, 252, 330, - 332, 342, 348, 358, 366, 372, 382, 392, 400, 410, 420, 424, 428, 262, 432, 436, - 446, 460, 470, 486, 496, 508, 516, 522, 536, 542, 548, 556, 568, 572, 580, 586, - 598, 608, 622, 634, 644, 648, 654, 662, 670, 682, 692, 700, 706, 708, 492, 198, - 714, 716, 726, 734, 742, 750, 760, 768, 782, 790, 794, 802, 204, 810, 820, 384, - 828, 836, 844, 850, 860, 864, 872, 880, 890, 894, 902, 908, 916, 920, 928, 934, - 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, - 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, - 944, 948, 960, 970, 986, 996, 1000, 1016, 1020, 1026, 1034, 1042, 364, 1046, 1056, 1058, - 1066, 1072, 1080, 932, 1092, 1100, 68, 1110, 1114, 1116, 1124, 1132, 1142, 1154, 1170, 1180, - 1192, 1204, 1218, 1234, 1248, 1264, 1276, 1290, 1300, 1310, 1322, 1334, 1342, 1350, 1360, 1370, - 1384, 1396, 1406, 1416, 1428, 1442, 1454, 1458, 1472, 1480, 1488, 1498, 1508, 1520, 1530, 1544, - 1556, 1568, 1582, 1598, 1612, 1628, 1642, 1654, 1666, 1678, 1692, 1706, 1710, 1714, 1720, 1726, - 1738, 1752, 1762, 1772, 1784, 1798, 1810, 1816, 1826, 1836, 1846, 1858, 1870, 1880, 1890, 65535, }; - -const unsigned char FontCharData[1902] = { - 0x00, 0x01, 0x20, 0x21, 0x03, 0x23, 0x23, 0x14, 0x14, 0x83, 0x00, 0x01, 0x20, 0x21, 0x04, 0x24, - 0x24, 0x13, 0x13, 0x84, 0x01, 0x21, 0x21, 0x23, 0x23, 0x14, 0x14, 0x03, 0x03, 0x01, 0x11, 0x92, - 0x11, 0x22, 0x22, 0x23, 0x23, 0x14, 0x14, 0x03, 0x03, 0x02, 0x02, 0x91, 0x01, 0x21, 0x21, 0x23, - 0x23, 0x01, 0x03, 0x21, 0x03, 0x01, 0x12, 0x94, 0x03, 0x23, 0x13, 0x14, 0x23, 0x22, 0x22, 0x11, - 0x11, 0x02, 0x02, 0x83, 0x12, 0x92, 0x12, 0x12, 0x01, 0x21, 0x21, 0x23, 0x23, 0x03, 0x03, 0x81, - 0x03, 0x21, 0x21, 0x22, 0x21, 0x11, 0x03, 0x14, 0x14, 0x23, 0x23, 0x92, 0x01, 0x10, 0x10, 0x21, - 0x21, 0x12, 0x12, 0x01, 0x12, 0x14, 0x03, 0xa3, 0x02, 0x03, 0x03, 0x13, 0x02, 0x12, 0x13, 0x10, - 0x10, 0xa1, 0x01, 0x23, 0x03, 0x21, 0x02, 0x11, 0x11, 0x22, 0x22, 0x13, 0x13, 0x82, 0x00, 0x22, - 0x22, 0x04, 0x04, 0x80, 0x20, 0x02, 0x02, 0x24, 0x24, 0xa0, 0x01, 0x10, 0x10, 0x21, 0x10, 0x14, - 0x14, 0x03, 0x14, 0xa3, 0x00, 0x03, 0x04, 0x04, 0x20, 0x23, 0x24, 0xa4, 0x00, 0x20, 0x00, 0x02, - 0x02, 0x22, 0x10, 0x14, 0x20, 0xa4, 0x01, 0x21, 0x21, 0x23, 0x23, 0x03, 0x03, 0x01, 0x20, 0x10, - 0x10, 0x14, 0x14, 0x84, 0x03, 0x23, 0x23, 0x24, 0x24, 0x04, 0x04, 0x83, 0x01, 0x10, 0x10, 0x21, - 0x10, 0x14, 0x14, 0x03, 0x14, 0x23, 0x04, 0xa4, 0x01, 0x10, 0x21, 0x10, 0x10, 0x94, 0x03, 0x14, - 0x23, 0x14, 0x10, 0x94, 0x02, 0x22, 0x22, 0x11, 0x22, 0x93, 0x02, 0x22, 0x02, 0x11, 0x02, 0x93, - 0x01, 0x02, 0x02, 0xa2, 0x02, 0x22, 0x22, 0x11, 0x11, 0x02, 0x02, 0x13, 0x13, 0xa2, 0x11, 0x22, - 0x22, 0x02, 0x02, 0x91, 0x02, 0x13, 0x13, 0x22, 0x22, 0x82, 0x10, 0x13, 0x14, 0x94, 0x10, 0x01, - 0x20, 0x91, 0x10, 0x14, 0x20, 0x24, 0x01, 0x21, 0x03, 0xa3, 0x21, 0x10, 0x10, 0x01, 0x01, 0x23, - 0x23, 0x14, 0x14, 0x03, 0x10, 0x94, 0x00, 0x01, 0x23, 0x24, 0x04, 0x03, 0x03, 0x21, 0x21, 0xa0, - 0x21, 0x10, 0x10, 0x01, 0x01, 0x12, 0x12, 0x03, 0x03, 0x14, 0x14, 0x23, 0x02, 0xa4, 0x10, 0x91, - 0x10, 0x01, 0x01, 0x03, 0x03, 0x94, 0x10, 0x21, 0x21, 0x23, 0x23, 0x94, 0x01, 0x23, 0x11, 0x13, - 0x21, 0x03, 0x02, 0xa2, 0x02, 0x22, 0x11, 0x93, 0x31, 0xc0, 0x03, 0xa1, 0x00, 0x20, 0x20, 0x24, - 0x24, 0x04, 0x04, 0x00, 0x12, 0x92, 0x01, 0x10, 0x10, 0x14, 0x04, 0xa4, 0x01, 0x10, 0x10, 0x21, - 0x21, 0x22, 0x22, 0x04, 0x04, 0xa4, 0x00, 0x20, 0x20, 0x24, 0x24, 0x04, 0x12, 0xa2, 0x00, 0x02, - 0x02, 0x22, 0x20, 0xa4, 0x20, 0x00, 0x00, 0x02, 0x02, 0x22, 0x22, 0x24, 0x24, 0x84, 0x20, 0x02, - 0x02, 0x22, 0x22, 0x24, 0x24, 0x04, 0x04, 0x82, 0x00, 0x20, 0x20, 0x21, 0x21, 0x12, 0x12, 0x94, - 0x00, 0x04, 0x00, 0x20, 0x20, 0x24, 0x04, 0x24, 0x02, 0xa2, 0x00, 0x02, 0x02, 0x22, 0x22, 0x20, - 0x20, 0x00, 0x22, 0x84, 0x11, 0x11, 0x13, 0x93, 0x11, 0x11, 0x13, 0x84, 0x20, 0x02, 0x02, 0xa4, - 0x00, 0x22, 0x22, 0x84, 0x01, 0x10, 0x10, 0x21, 0x21, 0x12, 0x12, 0x13, 0x14, 0x94, 0x21, 0x01, - 0x01, 0x04, 0x04, 0x24, 0x24, 0x22, 0x22, 0x12, 0x12, 0x13, 0x13, 0xa3, 0x04, 0x01, 0x01, 0x10, - 0x10, 0x21, 0x21, 0x24, 0x02, 0xa2, 0x00, 0x04, 0x04, 0x14, 0x14, 0x23, 0x23, 0x12, 0x12, 0x02, - 0x12, 0x21, 0x21, 0x10, 0x10, 0x80, 0x23, 0x14, 0x14, 0x03, 0x03, 0x01, 0x01, 0x10, 0x10, 0xa1, - 0x00, 0x10, 0x10, 0x21, 0x21, 0x23, 0x23, 0x14, 0x14, 0x04, 0x04, 0x80, 0x00, 0x04, 0x04, 0x24, - 0x00, 0x20, 0x02, 0x92, 0x00, 0x04, 0x00, 0x20, 0x02, 0x92, 0x21, 0x10, 0x10, 0x01, 0x01, 0x03, - 0x03, 0x14, 0x14, 0x23, 0x23, 0x22, 0x22, 0x92, 0x00, 0x04, 0x20, 0x24, 0x02, 0xa2, 0x00, 0x20, - 0x10, 0x14, 0x04, 0xa4, 0x00, 0x20, 0x20, 0x23, 0x23, 0x14, 0x14, 0x83, 0x00, 0x04, 0x02, 0x12, - 0x12, 0x21, 0x21, 0x20, 0x12, 0x23, 0x23, 0xa4, 0x00, 0x04, 0x04, 0xa4, 0x04, 0x00, 0x00, 0x11, - 0x11, 0x20, 0x20, 0xa4, 0x04, 0x00, 0x00, 0x22, 0x20, 0xa4, 0x01, 0x10, 0x10, 0x21, 0x21, 0x23, - 0x23, 0x14, 0x14, 0x03, 0x03, 0x81, 0x00, 0x04, 0x00, 0x10, 0x10, 0x21, 0x21, 0x12, 0x12, 0x82, - 0x01, 0x10, 0x10, 0x21, 0x21, 0x23, 0x23, 0x14, 0x14, 0x03, 0x03, 0x01, 0x04, 0x93, 0x00, 0x04, - 0x00, 0x10, 0x10, 0x21, 0x21, 0x12, 0x12, 0x02, 0x02, 0xa4, 0x21, 0x10, 0x10, 0x01, 0x01, 0x23, - 0x23, 0x14, 0x14, 0x83, 0x00, 0x20, 0x10, 0x94, 0x00, 0x04, 0x04, 0x24, 0x24, 0xa0, 0x00, 0x03, - 0x03, 0x14, 0x14, 0x23, 0x23, 0xa0, 0x00, 0x04, 0x04, 0x24, 0x14, 0x13, 0x24, 0xa0, 0x00, 0x01, - 0x01, 0x23, 0x23, 0x24, 0x04, 0x03, 0x03, 0x21, 0x21, 0xa0, 0x00, 0x01, 0x01, 0x12, 0x12, 0x14, - 0x12, 0x21, 0x21, 0xa0, 0x00, 0x20, 0x20, 0x02, 0x02, 0x04, 0x04, 0xa4, 0x10, 0x00, 0x00, 0x04, - 0x04, 0x94, 0x01, 0xa3, 0x10, 0x20, 0x20, 0x24, 0x24, 0x94, 0x00, 0x91, 0x02, 0x04, 0x04, 0x24, - 0x24, 0x22, 0x23, 0x12, 0x12, 0x82, 0x00, 0x04, 0x04, 0x24, 0x24, 0x22, 0x22, 0x82, 0x24, 0x04, - 0x04, 0x03, 0x03, 0x12, 0x12, 0xa2, 0x20, 0x24, 0x24, 0x04, 0x04, 0x02, 0x02, 0xa2, 0x24, 0x04, - 0x04, 0x02, 0x02, 0x22, 0x22, 0x23, 0x23, 0x93, 0x04, 0x01, 0x02, 0x12, 0x01, 0x10, 0x10, 0xa1, - 0x23, 0x12, 0x12, 0x03, 0x03, 0x14, 0x14, 0x23, 0x23, 0x24, 0x24, 0x15, 0x15, 0x84, 0x00, 0x04, - 0x03, 0x12, 0x12, 0x23, 0x23, 0xa4, 0x11, 0x11, 0x12, 0x94, 0x22, 0x22, 0x23, 0x24, 0x24, 0x15, - 0x15, 0x84, 0x00, 0x04, 0x03, 0x13, 0x13, 0x22, 0x13, 0xa4, 0x02, 0x04, 0x02, 0x13, 0x12, 0x14, - 0x12, 0x23, 0x23, 0xa4, 0x02, 0x04, 0x03, 0x12, 0x12, 0x23, 0x23, 0xa4, 0x02, 0x05, 0x04, 0x24, - 0x24, 0x22, 0x22, 0x82, 0x02, 0x04, 0x04, 0x24, 0x25, 0x22, 0x22, 0x82, 0x02, 0x04, 0x03, 0x12, - 0x12, 0xa2, 0x22, 0x02, 0x02, 0x03, 0x03, 0x23, 0x23, 0x24, 0x24, 0x84, 0x11, 0x14, 0x02, 0xa2, - 0x02, 0x04, 0x04, 0x14, 0x14, 0x23, 0x24, 0xa2, 0x02, 0x03, 0x03, 0x14, 0x14, 0x23, 0x23, 0xa2, - 0x02, 0x03, 0x03, 0x14, 0x14, 0x12, 0x13, 0x24, 0x24, 0xa2, 0x02, 0x24, 0x04, 0xa2, 0x02, 0x03, - 0x03, 0x14, 0x22, 0x23, 0x23, 0x85, 0x02, 0x22, 0x22, 0x04, 0x04, 0xa4, 0x20, 0x10, 0x10, 0x14, - 0x14, 0x24, 0x12, 0x82, 0x10, 0x11, 0x13, 0x94, 0x00, 0x10, 0x10, 0x14, 0x14, 0x04, 0x12, 0xa2, - 0x01, 0x10, 0x10, 0x11, 0x11, 0xa0, 0x03, 0x04, 0x04, 0x24, 0x24, 0x23, 0x23, 0x12, 0x12, 0x83, - 0x10, 0x10, 0x11, 0x94, 0x21, 0x10, 0x10, 0x01, 0x01, 0x02, 0x02, 0x13, 0x13, 0x22, 0x10, 0x93, - 0x11, 0x00, 0x00, 0x04, 0x04, 0x24, 0x24, 0x23, 0x02, 0x92, 0x01, 0x02, 0x11, 0x21, 0x22, 0x23, - 0x03, 0x13, 0x02, 0x11, 0x11, 0x22, 0x22, 0x13, 0x13, 0x82, 0x00, 0x11, 0x11, 0x20, 0x11, 0x14, - 0x02, 0x22, 0x03, 0xa3, 0x10, 0x12, 0x13, 0x95, 0x20, 0x00, 0x00, 0x02, 0x02, 0x11, 0x11, 0x22, - 0x02, 0x13, 0x13, 0x22, 0x22, 0x24, 0x24, 0x84, 0x00, 0x00, 0x20, 0xa0, 0x20, 0x10, 0x10, 0x11, - 0x11, 0xa1, 0x10, 0x21, 0x20, 0x21, 0x21, 0x11, 0x11, 0x90, 0x11, 0x02, 0x02, 0x13, 0x21, 0x12, - 0x12, 0xa3, 0x01, 0x21, 0x21, 0xa2, 0x10, 0x20, 0x20, 0x21, 0x21, 0x11, 0x12, 0x10, 0x11, 0xa2, - 0x00, 0xa0, 0x01, 0x10, 0x10, 0x21, 0x21, 0x12, 0x12, 0x81, 0x02, 0x22, 0x11, 0x13, 0x03, 0xa3, - 0x01, 0x10, 0x10, 0x21, 0x21, 0x03, 0x03, 0xa3, 0x01, 0x10, 0x10, 0x21, 0x21, 0x12, 0x12, 0x02, - 0x12, 0x23, 0x23, 0x83, 0x02, 0x05, 0x04, 0x14, 0x14, 0x23, 0x22, 0xa4, 0x14, 0x10, 0x10, 0x01, - 0x01, 0x12, 0x10, 0x20, 0x20, 0xa4, 0x14, 0x15, 0x15, 0x85, 0x20, 0xa1, 0x10, 0x20, 0x20, 0x21, - 0x21, 0x11, 0x11, 0x90, 0x01, 0x12, 0x12, 0x03, 0x11, 0x22, 0x22, 0x93, 0x00, 0x01, 0x02, 0x20, - 0x12, 0x13, 0x13, 0x23, 0x22, 0xa4, 0x00, 0x01, 0x02, 0x20, 0x12, 0x22, 0x22, 0x13, 0x13, 0x14, - 0x14, 0xa4, 0x00, 0x10, 0x10, 0x11, 0x11, 0x01, 0x11, 0x02, 0x02, 0x20, 0x12, 0x13, 0x13, 0x23, - 0x22, 0xa4, 0x10, 0x10, 0x11, 0x12, 0x12, 0x03, 0x03, 0x14, 0x14, 0xa3, 0x04, 0x02, 0x02, 0x11, - 0x11, 0x22, 0x22, 0x24, 0x03, 0x23, 0x10, 0xa1, 0x04, 0x02, 0x02, 0x11, 0x11, 0x22, 0x22, 0x24, - 0x03, 0x23, 0x01, 0x90, 0x04, 0x02, 0x02, 0x11, 0x11, 0x22, 0x22, 0x24, 0x03, 0x23, 0x01, 0x10, - 0x10, 0xa1, 0x04, 0x02, 0x02, 0x11, 0x11, 0x22, 0x22, 0x24, 0x03, 0x23, 0x01, 0x10, 0x10, 0x11, - 0x11, 0xa0, 0x04, 0x02, 0x02, 0x11, 0x11, 0x22, 0x22, 0x24, 0x03, 0x23, 0x00, 0x00, 0x20, 0xa0, - 0x04, 0x02, 0x02, 0x11, 0x11, 0x22, 0x22, 0x24, 0x03, 0x23, 0x00, 0x20, 0x20, 0x11, 0x11, 0x80, - 0x00, 0x04, 0x02, 0x22, 0x00, 0x11, 0x10, 0x14, 0x10, 0x20, 0x14, 0xa4, 0x23, 0x14, 0x14, 0x03, - 0x03, 0x01, 0x01, 0x10, 0x10, 0x21, 0x14, 0x15, 0x15, 0x85, 0x02, 0x22, 0x02, 0x04, 0x04, 0x24, - 0x03, 0x13, 0x00, 0x91, 0x02, 0x22, 0x02, 0x04, 0x04, 0x24, 0x03, 0x13, 0x11, 0xa0, 0x02, 0x22, - 0x02, 0x04, 0x04, 0x24, 0x03, 0x13, 0x01, 0x10, 0x10, 0xa1, 0x02, 0x22, 0x02, 0x04, 0x04, 0x24, - 0x03, 0x13, 0x00, 0x00, 0x20, 0xa0, 0x02, 0x22, 0x12, 0x14, 0x04, 0x24, 0x00, 0x91, 0x02, 0x22, - 0x12, 0x14, 0x04, 0x24, 0x11, 0xa0, 0x02, 0x22, 0x12, 0x14, 0x04, 0x24, 0x01, 0x10, 0x10, 0xa1, - 0x02, 0x22, 0x12, 0x14, 0x04, 0x24, 0x20, 0x20, 0x00, 0x80, 0x00, 0x10, 0x10, 0x21, 0x21, 0x23, - 0x23, 0x14, 0x14, 0x04, 0x04, 0x00, 0x02, 0x92, 0x04, 0x02, 0x02, 0x24, 0x24, 0x22, 0x01, 0x10, - 0x10, 0x11, 0x11, 0xa0, 0x02, 0x22, 0x22, 0x24, 0x24, 0x04, 0x04, 0x02, 0x00, 0x91, 0x02, 0x22, - 0x22, 0x24, 0x24, 0x04, 0x04, 0x02, 0x11, 0xa0, 0x02, 0x22, 0x22, 0x24, 0x24, 0x04, 0x04, 0x02, - 0x11, 0x20, 0x00, 0x91, 0x02, 0x22, 0x22, 0x24, 0x24, 0x04, 0x04, 0x02, 0x01, 0x10, 0x10, 0x11, - 0x11, 0xa0, 0x01, 0x21, 0x21, 0x24, 0x24, 0x04, 0x04, 0x01, 0x00, 0x00, 0x20, 0xa0, 0x01, 0x23, - 0x03, 0xa1, 0x01, 0x10, 0x10, 0x21, 0x21, 0x23, 0x23, 0x14, 0x14, 0x03, 0x03, 0x01, 0x03, 0xa1, - 0x01, 0x04, 0x04, 0x24, 0x24, 0x21, 0x11, 0xa0, 0x01, 0x04, 0x04, 0x24, 0x24, 0x21, 0x00, 0x91, - 0x02, 0x04, 0x04, 0x24, 0x24, 0x22, 0x01, 0x10, 0x10, 0xa1, 0x01, 0x04, 0x04, 0x24, 0x24, 0x21, - 0x00, 0x00, 0x20, 0xa0, 0x01, 0x02, 0x02, 0x13, 0x13, 0x14, 0x13, 0x22, 0x22, 0x21, 0x11, 0xa0, - 0x00, 0x04, 0x01, 0x11, 0x11, 0x22, 0x22, 0x13, 0x13, 0x83, 0x00, 0x05, 0x00, 0x10, 0x10, 0x21, - 0x21, 0x12, 0x02, 0x22, 0x22, 0x24, 0x24, 0x84, 0x02, 0x04, 0x04, 0x24, 0x24, 0x22, 0x23, 0x12, - 0x12, 0x02, 0x00, 0x91, 0x02, 0x04, 0x04, 0x24, 0x24, 0x22, 0x23, 0x12, 0x12, 0x02, 0x11, 0xa0, - 0x02, 0x04, 0x04, 0x24, 0x24, 0x22, 0x23, 0x12, 0x12, 0x02, 0x01, 0x10, 0x10, 0xa1, 0x02, 0x04, - 0x04, 0x24, 0x24, 0x22, 0x23, 0x12, 0x12, 0x02, 0x01, 0x10, 0x10, 0x11, 0x11, 0xa0, 0x02, 0x04, - 0x04, 0x24, 0x24, 0x22, 0x23, 0x12, 0x12, 0x02, 0x01, 0x01, 0x21, 0xa1, 0x02, 0x04, 0x04, 0x24, - 0x24, 0x22, 0x23, 0x12, 0x12, 0x02, 0x01, 0x10, 0x10, 0x21, 0x21, 0x81, 0x02, 0x13, 0x02, 0x04, - 0x04, 0x24, 0x12, 0x14, 0x12, 0x22, 0x13, 0x23, 0x22, 0xa3, 0x03, 0x04, 0x04, 0x24, 0x03, 0x12, - 0x12, 0x22, 0x14, 0x15, 0x15, 0x85, 0x24, 0x04, 0x04, 0x02, 0x02, 0x22, 0x22, 0x23, 0x23, 0x13, - 0x00, 0x91, 0x24, 0x04, 0x04, 0x02, 0x02, 0x22, 0x22, 0x23, 0x23, 0x13, 0x11, 0xa0, 0x24, 0x04, - 0x04, 0x02, 0x02, 0x22, 0x22, 0x23, 0x23, 0x13, 0x01, 0x10, 0x10, 0xa1, 0x24, 0x04, 0x04, 0x02, - 0x02, 0x22, 0x22, 0x23, 0x23, 0x13, 0x01, 0x01, 0x21, 0xa1, 0x12, 0x14, 0x00, 0x91, 0x12, 0x14, - 0x11, 0xa0, 0x12, 0x14, 0x01, 0x10, 0x10, 0xa1, 0x12, 0x14, 0x01, 0x01, 0x21, 0xa1, 0x00, 0x22, - 0x11, 0x20, 0x02, 0x22, 0x22, 0x24, 0x24, 0x04, 0x04, 0x82, 0x02, 0x04, 0x03, 0x12, 0x12, 0x23, - 0x23, 0x24, 0x01, 0x10, 0x10, 0x11, 0x11, 0xa0, 0x02, 0x04, 0x04, 0x24, 0x24, 0x22, 0x22, 0x02, - 0x00, 0x91, 0x02, 0x04, 0x04, 0x24, 0x24, 0x22, 0x22, 0x02, 0x11, 0xa0, 0x02, 0x04, 0x04, 0x24, - 0x24, 0x22, 0x22, 0x02, 0x01, 0x10, 0x10, 0xa1, 0x02, 0x04, 0x04, 0x24, 0x24, 0x22, 0x22, 0x02, - 0x01, 0x10, 0x10, 0x11, 0x11, 0xa0, 0x02, 0x04, 0x04, 0x24, 0x24, 0x22, 0x22, 0x02, 0x01, 0x01, - 0x21, 0xa1, 0x11, 0x11, 0x02, 0x22, 0x13, 0x93, 0x02, 0x04, 0x04, 0x24, 0x24, 0x22, 0x22, 0x02, - 0x04, 0xa2, 0x02, 0x04, 0x04, 0x14, 0x14, 0x23, 0x24, 0x22, 0x00, 0x91, 0x02, 0x04, 0x04, 0x14, - 0x14, 0x23, 0x24, 0x22, 0x11, 0xa0, 0x02, 0x04, 0x04, 0x14, 0x14, 0x23, 0x24, 0x22, 0x01, 0x10, - 0x10, 0xa1, 0x02, 0x04, 0x04, 0x14, 0x14, 0x23, 0x24, 0x22, 0x01, 0x01, 0x21, 0xa1, 0x02, 0x03, - 0x03, 0x14, 0x22, 0x23, 0x23, 0x05, 0x11, 0xa0, 0x00, 0x04, 0x02, 0x11, 0x11, 0x22, 0x22, 0x13, - 0x13, 0x82, 0x02, 0x03, 0x03, 0x14, 0x22, 0x23, 0x23, 0x05, 0x01, 0x01, 0x21, 0xa1, }; - - - -void CNFGDrawText( const char * text, int scale ) -{ - const unsigned char * lmap; - float iox = (float)CNFGPenX; - float ioy = (float)CNFGPenY; - - int place = 0; - unsigned short index; - int bQuit = 0; - while( text[place] ) - { - unsigned char c = text[place]; - - switch( c ) - { - case 9: - iox += 12 * scale; - break; - case 10: - iox = (float)CNFGPenX; - ioy += 6 * scale; - break; - default: - index = FontCharMap[c]; - if( index == 65535 ) - { - iox += 3 * scale; - break; - } - - lmap = &FontCharData[index]; - do - { - int x1 = (int)((((*lmap) & 0x70)>>4)*scale + iox); - int y1 = (int)(((*lmap) & 0x0f)*scale + ioy); - int x2 = (int)((((*(lmap+1)) & 0x70)>>4)*scale + iox); - int y2 = (int)(((*(lmap+1)) & 0x0f)*scale + ioy); - lmap++; - if(x1 == x2 && y1 == y2){ - CNFGTackPixel( x1, y1 ); - } else { - CNFGTackSegment( x1, y1, x2, y2 ); - } - bQuit = *lmap & 0x80; - lmap++; - } while( !bQuit ); - - iox += 3 * scale; - } - place++; - } -} - - -void CNFGDrawBox( int x1, int y1, int x2, int y2 ) -{ - unsigned lc = CNFGLastColor; - CNFGColor( CNFGDialogColor ); - CNFGTackRectangle( x1, y1, x2, y2 ); - CNFGColor( lc ); - CNFGTackSegment( x1, y1, x2, y1 ); - CNFGTackSegment( x2, y1, x2, y2 ); - CNFGTackSegment( x2, y2, x1, y2 ); - CNFGTackSegment( x1, y2, x1, y1 ); -} - -void CNFGGetTextExtents( const char * text, int * w, int * h, int textsize ) -{ - int charsx = 0; - int charsy = 1; - int charsline = 0; - const char * s; - - for( s = text; *s; s++ ) - { - if( *s == '\n' ) - { - charsline = 0; - if( *(s+1) ) - charsy++; - } - else - { - charsline++; - if( charsline > charsx ) - charsx = charsline; - } - } - - *w = charsx * textsize * 3 + textsize; - *h = charsy * textsize * 6; -} - -void CNFGDrawTextbox( int x, int y, const char * text, int textsize ) -{ - int w; - int h; - - CNFGGetTextExtents( text, &w, &h, textsize ); - - CNFGDrawBox( x, y, x + w, y + h ); - CNFGPenX = x + textsize; - CNFGPenY = y + textsize; - CNFGDrawText( text, textsize ); -} diff --git a/redist/DrawFunctions.h b/redist/DrawFunctions.h deleted file mode 100644 index 542fcf9..0000000 --- a/redist/DrawFunctions.h +++ /dev/null @@ -1,55 +0,0 @@ -//Copyright (c) 2011 <>< Charles Lohr - Under the MIT/x11 or NewBSD License you choose. - -#ifndef _DRAWFUCNTIONS_H -#define _DRAWFUCNTIONS_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include - -typedef struct { - short x, y; -} RDPoint; - -extern int CNFGPenX, CNFGPenY; -extern uint32_t CNFGBGColor; -extern uint32_t CNFGLastColor; -extern uint32_t CNFGDialogColor; //background for boxes - -void CNFGDrawText( const char * text, int scale ); -void CNFGDrawBox( int x1, int y1, int x2, int y2 ); -void CNFGGetTextExtents( const char * text, int * w, int * h, int textsize ); -void CNFGDrawTextbox( int x, int y, const char * text, int textsize ); //ignores pen. - -//To be provided by driver. -uint32_t CNFGColor( uint32_t RGB ); -void CNFGUpdateScreenWithBitmap( unsigned long * data, int w, int h ); -void CNFGTackPixel( short x1, short y1 ); -void CNFGTackSegment( short x1, short y1, short x2, short y2 ); -void CNFGTackRectangle( short x1, short y1, short x2, short y2 ); -void CNFGTackPoly( RDPoint * points, int verts ); -void CNFGClearFrame(); -void CNFGSwapBuffers(); - -void CNFGGetDimensions( short * x, short * y ); -void CNFGTearDown(); -void CNFGSetup( const char * WindowName, int w, int h ); -void CNFGSetupFullscreen( const char * WindowName, int screen_number ); -void CNFGHandleInput(); - - -//You must provide: -void HandleKey( int keycode, int bDown ); -void HandleButton( int x, int y, int button, int bDown ); -void HandleMotion( int x, int y, int mask ); - - -#ifdef __cplusplus -}; -#endif - - -#endif - diff --git a/redist/RawDrawNull.c b/redist/RawDrawNull.c deleted file mode 100644 index 34346cc..0000000 --- a/redist/RawDrawNull.c +++ /dev/null @@ -1,74 +0,0 @@ -//Copyright (c) 2017 <>< Charles Lohr - Under the MIT/x11 or NewBSD License you choose. - -#include "DrawFunctions.h" - -static int w, h; -void CNFGGetDimensions( short * x, short * y ) -{ - *x = w; - *y = h; -} - -static void InternalLinkScreenAndGo( const char * WindowName ) -{ -} - -void CNFGSetupFullscreen( const char * WindowName, int screen_no ) -{ - CNFGSetup( WindowName, 640, 480 ); -} - - -void CNFGTearDown() -{ -} - -void CNFGSetup( const char * WindowName, int sw, int sh ) -{ - w = sw; - h = sh; -} - -void CNFGHandleInput() -{ -} - - -void CNFGUpdateScreenWithBitmap( unsigned long * data, int w, int h ) -{ -} - - -#ifndef RASTERIZER - - -uint32_t CNFGColor( uint32_t RGB ) -{ -} - -void CNFGClearFrame() -{ -} - -void CNFGSwapBuffers() -{ -} - -void CNFGTackSegment( short x1, short y1, short x2, short y2 ) -{ -} - -void CNFGTackPixel( short x1, short y1 ) -{ -} - -void CNFGTackRectangle( short x1, short y1, short x2, short y2 ) -{ -} - -void CNFGTackPoly( RDPoint * points, int verts ) -{ -} - -#endif - diff --git a/redist/RawDrawRasterizer.c b/redist/RawDrawRasterizer.c deleted file mode 100644 index af40588..0000000 --- a/redist/RawDrawRasterizer.c +++ /dev/null @@ -1,238 +0,0 @@ -#ifdef RASTERIZER -#include "DrawFunctions.h" -#include -#include -#include - -static uint32_t * buffer = 0; -static short bufferx; -static short buffery; - -static uint32_t SWAPS( uint32_t r ) -{ - uint32_t ret = (r&0xFF)<<16; - r>>=8; - ret |= (r&0xff)<<8; - r>>=8; - ret |= r; - return ret; -} - -uint32_t CNFGColor( uint32_t RGB ) -{ - CNFGLastColor = SWAPS(RGB); - return CNFGLastColor; -} - -void CNFGTackPixel( short x, short y ) -{ - buffer[bufferx*y+x] = CNFGLastColor; -} - -void CNFGTackSegment( short x1, short y1, short x2, short y2 ) -{ - short tx, ty; - float slope, lp; - - short dx = x2 - x1; - short dy = y2 - y1; - - if( !buffer ) return; - - if( dx < 0 ) dx = -dx; - if( dy < 0 ) dy = -dy; - - if( dx > dy ) - { - short minx = (x1 < x2)?x1:x2; - short maxx = (x1 < x2)?x2:x1; - short miny = (x1 < x2)?y1:y2; - short maxy = (x1 < x2)?y2:y1; - float thisy = miny; - slope = (float)(maxy-miny) / (float)(maxx-minx); - - for( tx = minx; tx <= maxx; tx++ ) - { - thisy += slope; - ty = thisy; - if( tx < 0 || ty < 0 || ty >= buffery ) continue; - if( tx >= bufferx ) break; - buffer[ty * bufferx + tx] = CNFGLastColor; - } - } - else - { - short minx = (y1 < y2)?x1:x2; - short maxx = (y1 < y2)?x2:x1; - short miny = (y1 < y2)?y1:y2; - short maxy = (y1 < y2)?y2:y1; - float thisx = minx; - slope = (float)(maxx-minx) / (float)(maxy-miny); - - for( ty = miny; ty <= maxy; ty++ ) - { - thisx += slope; - tx = thisx; - if( ty < 0 || tx < 0 || tx >= bufferx ) continue; - if( ty >= buffery ) break; - buffer[ty * bufferx + tx] = CNFGLastColor; - } - } -} - -void CNFGTackRectangle( short x1, short y1, short x2, short y2 ) -{ - short minx = (x1=x2)?x1:x2; - short maxy = (y1>=y2)?y1:y2; - - short x, y; - - if( minx < 0 ) minx = 0; - if( miny < 0 ) miny = 0; - if( maxx >= bufferx ) maxx = bufferx-1; - if( maxy >= buffery ) maxy = buffery-1; - - for( y = miny; y <= maxy; y++ ) - { - uint32_t * bufferstart = &buffer[y * bufferx + minx]; - for( x = minx; x <= maxx; x++ ) - { - (*bufferstart++) = CNFGLastColor; - } - } -} - -void CNFGTackPoly( RDPoint * points, int verts ) -{ - short minx = 10000, miny = 10000; - short maxx =-10000, maxy =-10000; - short i, x, y; - - //Just in case... - if( verts > 32767 ) return; - - for( i = 0; i < verts; i++ ) - { - RDPoint * p = &points[i]; - if( p->x < minx ) minx = p->x; - if( p->y < miny ) miny = p->y; - if( p->x > maxx ) maxx = p->x; - if( p->y > maxy ) maxy = p->y; - } - - if( miny < 0 ) miny = 0; - if( maxy >= buffery ) maxy = buffery-1; - - for( y = miny; y <= maxy; y++ ) - { - short startfillx = maxx; - short endfillx = minx; - - //Figure out what line segments intersect this line. - for( i = 0; i < verts; i++ ) - { - short pl = i + 1; - if( pl == verts ) pl = 0; - - RDPoint ptop; - RDPoint pbot; - - ptop.x = points[i].x; - ptop.y = points[i].y; - pbot.x = points[pl].x; - pbot.y = points[pl].y; -//printf( "Poly: %d %d\n", pbot.y, ptop.y ); - - if( pbot.y < ptop.y ) - { - RDPoint ptmp; - ptmp.x = pbot.x; - ptmp.y = pbot.y; - pbot.x = ptop.x; - pbot.y = ptop.y; - ptop.x = ptmp.x; - ptop.y = ptmp.y; - } - - //Make sure this line segment is within our range. -//printf( "PT: %d %d %d\n", y, ptop.y, pbot.y ); - if( ptop.y <= y && pbot.y >= y ) - { - short diffy = pbot.y - ptop.y; - uint32_t placey = (uint32_t)(y - ptop.y)<<16; //Scale by 16 so we can do integer math. - short diffx = pbot.x - ptop.x; - short isectx; - - if( diffy == 0 ) - { - if( pbot.x < ptop.x ) - { - if( startfillx > pbot.x ) startfillx = pbot.x; - if( endfillx < ptop.x ) endfillx = ptop.x; - } - else - { - if( startfillx > ptop.x ) startfillx = ptop.x; - if( endfillx < pbot.x ) endfillx = pbot.x; - } - } - else - { - //Inner part is scaled by 65536, outer part must be scaled back. - isectx = (( (placey / diffy) * diffx + 32768 )>>16) + ptop.x; - if( isectx < startfillx ) startfillx = isectx; - if( isectx > endfillx ) endfillx = isectx; - } -//printf( "R: %d %d %d\n", pbot.x, ptop.x, isectx ); - } - } - -//printf( "%d %d %d\n", y, startfillx, endfillx ); - - if( endfillx >= bufferx ) endfillx = bufferx - 1; - if( endfillx >= bufferx ) endfillx = buffery - 1; - if( startfillx < 0 ) startfillx = 0; - if( startfillx < 0 ) startfillx = 0; - - unsigned int * bufferstart = &buffer[y * bufferx + startfillx]; - for( x = startfillx; x <= endfillx; x++ ) - { - (*bufferstart++) = CNFGLastColor; - } - } -//exit(1); -} - - -void CNFGClearFrame() -{ - int i, m; - uint32_t col = 0; - short x, y; - CNFGGetDimensions( &x, &y ); - if( x != bufferx || y != buffery || !buffer ) - { - bufferx = x; - buffery = y; - printf( "MALLOCING: %d\n", x * y ); - buffer = malloc( x * y * 8 ); - } - - m = x * y; - col = CNFGColor( CNFGBGColor ); - for( i = 0; i < m; i++ ) - { -//printf( "Got: %d %p %d\n", m, buffer, i ); - buffer[i] = col; - } -} - -void CNFGSwapBuffers() -{ - CNFGUpdateScreenWithBitmap( buffer, bufferx, buffery ); -} - - -#endif diff --git a/redist/WinDriver.c b/redist/WinDriver.c deleted file mode 100644 index 3613150..0000000 --- a/redist/WinDriver.c +++ /dev/null @@ -1,231 +0,0 @@ -//Copyright (c) 2011 <>< Charles Lohr - Under the MIT/x11 or NewBSD License you choose. -//Portion from: http://en.wikibooks.org/wiki/Windows_Programming/Window_Creation - - -#include "DrawFunctions.h" -#include -#include -#include //for alloca - -static HINSTANCE lhInstance; -static HWND lsHWND; -static HDC lsHDC; -static HBITMAP lsBackBitmap; -static HDC lsWindowHDC; -static HBRUSH lsHBR; -static HPEN lsHPEN; -static HBRUSH lsClearBrush; -static unsigned int lsLastWidth; -static unsigned int lsLastHeight; - -static void InternalHandleResize() -{ - DeleteObject( lsBackBitmap ); - lsBackBitmap = CreateCompatibleBitmap( lsHDC, lsLastWidth, lsLastHeight ); - SelectObject( lsHDC, lsBackBitmap ); - -} - -uint32_t CNFGColor( uint32_t RGB ) -{ - CNFGLastColor = RGB; - - DeleteObject( lsHBR ); - lsHBR = CreateSolidBrush( RGB ); - SelectObject( lsHDC, lsHBR ); - - DeleteObject( lsHPEN ); - lsHPEN = CreatePen( PS_SOLID, 0, RGB ); - SelectObject( lsHDC, lsHPEN ); - - return RGB; -} - -void CNFGTackSegment( short x1, short y1, short x2, short y2 ) -{ - POINT pt[2] = { {x1, y1}, {x2, y2} }; - Polyline( lsHDC, pt, 2 ); - SetPixel( lsHDC, x1, y1, CNFGLastColor ); - SetPixel( lsHDC, x2, y2, CNFGLastColor ); -} - -void CNFGTackRectangle( short x1, short y1, short x2, short y2 ) -{ - RECT r; - if( x1 < x2 ) { r.left = x1; r.right = x2; } - else { r.left = x2; r.right = x1; } - if( y1 < y2 ) { r.top = y1; r.bottom = y2; } - else { r.top = y2; r.bottom = y1; } - FillRect( lsHDC, &r, lsHBR ); -} - -void CNFGClearFrame() -{ - RECT r = { 0, 0, lsLastWidth, lsLastHeight }; - DeleteObject( lsClearBrush ); - lsClearBrush = CreateSolidBrush( CNFGBGColor ); - SelectObject( lsHDC, lsClearBrush ); - - FillRect( lsHDC, &r, lsClearBrush ); -} - -void CNFGSwapBuffers() -{ - int thisw, thish; - RECT r; - BitBlt( lsWindowHDC, 0, 0, lsLastWidth, lsLastHeight, lsHDC, 0, 0, SRCCOPY ); - UpdateWindow( lsHWND ); - - //Check to see if the window is closed. - if( !IsWindow( lsHWND ) ) - { - exit( 0 ); - } - - GetClientRect( lsHWND, &r ); - thisw = r.right - r.left; - thish = r.bottom - r.top; - if( thisw != lsLastWidth || thish != lsLastHeight ) - { - lsLastWidth = thisw; - lsLastHeight = thish; - InternalHandleResize(); - } -} - -void CNFGTackPoly( RDPoint * points, int verts ) -{ - int i; - POINT * t = (POINT*)alloca( sizeof( POINT ) * verts ); - for( i = 0; i < verts; i++ ) - { - t[i].x = points[i].x; - t[i].y = points[i].y; - } - Polygon( lsHDC, t, verts ); -} - - -void CNFGTackPixel( short x1, short y1 ) -{ - SetPixel( lsHDC, x1, y1, CNFGLastColor ); -} - -void CNFGGetDimensions( short * x, short * y ) -{ - *x = lsLastWidth; - *y = lsLastHeight; -} - -//This was from the article -LRESULT CALLBACK MyWndProc(HWND hwnd, UINT msg, WPARAM wParam, LPARAM lParam) -{ - switch(msg) - { - case WM_DESTROY: - CNFGTearDown(); - return 0; - } - return DefWindowProc(hwnd, msg, wParam, lParam); -} - -void CNFGTearDown() -{ - PostQuitMessage(0); -} - -//This was from the article, too... well, mostly. -void CNFGSetup( const char * name_of_window, int width, int height ) -{ - static LPSTR szClassName = "MyClass"; - RECT client, window; - WNDCLASS wnd; - int w, h, wd, hd; - HINSTANCE hInstance = GetModuleHandle(NULL); - - lsLastWidth = width; - lsLastHeight = height; - - wnd.style = CS_HREDRAW | CS_VREDRAW; //we will explain this later - wnd.lpfnWndProc = MyWndProc; - wnd.cbClsExtra = 0; - wnd.cbWndExtra = 0; - wnd.hInstance = hInstance; - wnd.hIcon = LoadIcon(NULL, IDI_APPLICATION); //default icon - wnd.hCursor = LoadCursor(NULL, IDC_ARROW); //default arrow mouse cursor - wnd.hbrBackground = (HBRUSH)(COLOR_BACKGROUND); - wnd.lpszMenuName = NULL; //no menu - wnd.lpszClassName = szClassName; - - if(!RegisterClass(&wnd)) //register the WNDCLASS - { - MessageBox(NULL, "This Program Requires Windows NT", "Error", MB_OK); - } - - lsHWND = CreateWindow(szClassName, - name_of_window, //name_of_window, - WS_OVERLAPPEDWINDOW, //basic window style - CW_USEDEFAULT, - CW_USEDEFAULT, //set starting point to default value - lsLastWidth, - lsLastHeight, //set all the dimensions to default value - NULL, //no parent window - NULL, //no menu - hInstance, - NULL); //no parameters to pass - - - lsWindowHDC = GetDC( lsHWND ); - lsHDC = CreateCompatibleDC( lsWindowHDC ); - lsBackBitmap = CreateCompatibleBitmap( lsWindowHDC, lsLastWidth, lsLastHeight ); - SelectObject( lsHDC, lsBackBitmap ); - - lsClearBrush = CreateSolidBrush( CNFGBGColor ); - lsHBR = CreateSolidBrush( 0xFFFFFF ); - lsHPEN = CreatePen( PS_SOLID, 0, 0xFFFFFF ); - - ShowWindow(lsHWND, 1); //display the window on the screen - - //Once set up... we have to change the window's borders so we get the client size right. - GetClientRect( lsHWND, &client ); - GetWindowRect( lsHWND, &window ); - w = ( window.right - window.left); - h = ( window.bottom - window.top); - wd = w - client.right; - hd = h - client.bottom; - MoveWindow( lsHWND, window.left, window.top, lsLastWidth + wd, lsLastHeight + hd, 1 ); - - InternalHandleResize(); -} - -void CNFGHandleInput() -{ - int ldown = 0; - - MSG msg; - while( PeekMessage( &msg, lsHWND, 0, 0xFFFF, 1 ) ) - { - TranslateMessage(&msg); - - switch( msg.message ) - { - case WM_MOUSEMOVE: - HandleMotion( (msg.lParam & 0xFFFF), (msg.lParam>>16) & 0xFFFF, ( (msg.wParam & 0x01)?1:0) | ((msg.wParam & 0x02)?2:0) | ((msg.wParam & 0x10)?4:0) ); - break; - case WM_LBUTTONDOWN: HandleButton( (msg.lParam & 0xFFFF), (msg.lParam>>16) & 0xFFFF, 1, 1 ); break; - case WM_RBUTTONDOWN: HandleButton( (msg.lParam & 0xFFFF), (msg.lParam>>16) & 0xFFFF, 2, 1 ); break; - case WM_MBUTTONDOWN: HandleButton( (msg.lParam & 0xFFFF), (msg.lParam>>16) & 0xFFFF, 3, 1 ); break; - case WM_LBUTTONUP: HandleButton( (msg.lParam & 0xFFFF), (msg.lParam>>16) & 0xFFFF, 1, 0 ); break; - case WM_RBUTTONUP: HandleButton( (msg.lParam & 0xFFFF), (msg.lParam>>16) & 0xFFFF, 2, 0 ); break; - case WM_MBUTTONUP: HandleButton( (msg.lParam & 0xFFFF), (msg.lParam>>16) & 0xFFFF, 3, 0 ); break; - case WM_KEYDOWN: - case WM_KEYUP: - HandleKey( tolower( (int)msg.wParam ), (msg.message==WM_KEYDOWN) ); - break; - default: - DispatchMessage(&msg); - break; - } - } -} - diff --git a/redist/XDriver.c b/redist/XDriver.c deleted file mode 100644 index 507ca95..0000000 --- a/redist/XDriver.c +++ /dev/null @@ -1,290 +0,0 @@ -//Copyright (c) 2011 <>< Charles Lohr - Under the MIT/x11 or NewBSD License you choose. -//portions from -//http://www.xmission.com/~georgeps/documentation/tutorials/Xlib_Beginner.html - -//#define HAS_XINERAMA - -#include "DrawFunctions.h" - -#include -#include -#include -#include -#include -#ifdef HAS_XINERAMA -#include -#include -#endif -#include -#include - -XWindowAttributes CNFGWinAtt; -XClassHint *CNFGClassHint; -Display *CNFGDisplay; -Window CNFGWindow; -Pixmap CNFGPixmap; -GC CNFGGC; -GC CNFGWindowGC; -int FullScreen = 0; - -void CNFGGetDimensions( short * x, short * y ) -{ - *x = CNFGWinAtt.width; - *y = CNFGWinAtt.height; -} - -static void InternalLinkScreenAndGo( const char * WindowName ) -{ - XGetWindowAttributes( CNFGDisplay, CNFGWindow, &CNFGWinAtt ); - - XGetClassHint( CNFGDisplay, CNFGWindow, CNFGClassHint ); - if (!CNFGClassHint) { - CNFGClassHint = XAllocClassHint(); - if (CNFGClassHint) { - CNFGClassHint->res_name = "cnping"; - CNFGClassHint->res_class = "cnping"; - XSetClassHint( CNFGDisplay, CNFGWindow, CNFGClassHint ); - } else { - fprintf( stderr, "Failed to allocate XClassHint!\n" ); - } - } else { - fprintf( stderr, "Pre-existing XClassHint\n" ); - } - - XSelectInput (CNFGDisplay, CNFGWindow, KeyPressMask | KeyReleaseMask | ButtonPressMask | ButtonReleaseMask | ExposureMask | PointerMotionMask ); - XSetStandardProperties( CNFGDisplay, CNFGWindow, WindowName, WindowName, None, NULL, 0, NULL ); - - CNFGWindowGC = XCreateGC(CNFGDisplay, CNFGWindow, 0, 0); - - CNFGPixmap = XCreatePixmap( CNFGDisplay, CNFGWindow, CNFGWinAtt.width, CNFGWinAtt.height, CNFGWinAtt.depth ); - CNFGGC = XCreateGC(CNFGDisplay, CNFGPixmap, 0, 0); -} - -void CNFGSetupFullscreen( const char * WindowName, int screen_no ) -{ -#ifdef HAS_XINERAMA - XineramaScreenInfo *screeninfo = NULL; - int screens; - int event_basep, error_basep, a, b; - CNFGDisplay = XOpenDisplay(NULL); - int screen = XDefaultScreen(CNFGDisplay); - int xpos, ypos; - - if (!XShapeQueryExtension(CNFGDisplay, &event_basep, &error_basep)) - { - fprintf( stderr, "X-Server does not support shape extension" ); - exit( 1 ); - } - - Visual * visual = DefaultVisual(CNFGDisplay, screen); - CNFGWinAtt.depth = DefaultDepth(CNFGDisplay, screen); - - if (XineramaQueryExtension(CNFGDisplay, &a, &b ) && - (screeninfo = XineramaQueryScreens(CNFGDisplay, &screens)) && - XineramaIsActive(CNFGDisplay) && screen_no >= 0 && - screen_no < screens ) { - - CNFGWinAtt.width = screeninfo[screen_no].width; - CNFGWinAtt.height = screeninfo[screen_no].height; - xpos = screeninfo[screen_no].x_org; - ypos = screeninfo[screen_no].y_org; - } else - { - CNFGWinAtt.width = XDisplayWidth(CNFGDisplay, screen); - CNFGWinAtt.height = XDisplayHeight(CNFGDisplay, screen); - xpos = 0; - ypos = 0; - } - if (screeninfo) - XFree(screeninfo); - - - XSetWindowAttributes setwinattr; - setwinattr.override_redirect = 1; - setwinattr.save_under = 1; - setwinattr.event_mask = StructureNotifyMask | SubstructureNotifyMask | ExposureMask | ButtonPressMask | ButtonReleaseMask | ButtonPressMask | PointerMotionMask | ButtonMotionMask | EnterWindowMask | LeaveWindowMask |KeyPressMask |KeyReleaseMask | SubstructureNotifyMask | FocusChangeMask; - setwinattr.border_pixel = 0; - - CNFGWindow = XCreateWindow(CNFGDisplay, XRootWindow(CNFGDisplay, screen), - xpos, ypos, CNFGWinAtt.width, CNFGWinAtt.height, - 0, CNFGWinAtt.depth, InputOutput, visual, CWBorderPixel | CWEventMask | CWOverrideRedirect | CWSaveUnder, &setwinattr); - - XMapWindow(CNFGDisplay, CNFGWindow); - XSetInputFocus( CNFGDisplay, CNFGWindow, RevertToParent, CurrentTime ); - XFlush(CNFGDisplay); - FullScreen = 1; -//printf( "%d %d %d %d\n", xpos, ypos, CNFGWinAtt.width, CNFGWinAtt.height ); - InternalLinkScreenAndGo( WindowName ); -/* - setwinattr.override_redirect = 1; - XChangeWindowAttributes( - CNFGDisplay, CNFGWindow, - CWBorderPixel | CWEventMask | CWOverrideRedirect, &setwinattr); -*/ -#else - CNFGSetup( WindowName, 640, 480 ); -#endif -} - - -void CNFGTearDown() -{ - if ( CNFGClassHint ) XFree( CNFGClassHint ); - if ( CNFGGC ) XFreeGC( CNFGDisplay, CNFGGC ); - if ( CNFGWindowGC ) XFreeGC( CNFGDisplay, CNFGWindowGC ); - if ( CNFGDisplay ) XCloseDisplay( CNFGDisplay ); - CNFGDisplay = NULL; - CNFGWindowGC = CNFGGC = NULL; - CNFGClassHint = NULL; -} - -void CNFGSetup( const char * WindowName, int w, int h ) -{ - CNFGDisplay = XOpenDisplay(NULL); - atexit( CNFGTearDown ); - XGetWindowAttributes( CNFGDisplay, RootWindow(CNFGDisplay, 0), &CNFGWinAtt ); - - int depth = CNFGWinAtt.depth; - CNFGWindow = XCreateWindow(CNFGDisplay, RootWindow(CNFGDisplay, 0), 1, 1, w, h, 0, depth, InputOutput, CopyFromParent, 0, 0 ); - XMapWindow(CNFGDisplay, CNFGWindow); - XFlush(CNFGDisplay); - - InternalLinkScreenAndGo( WindowName ); - - Atom WM_DELETE_WINDOW = XInternAtom( CNFGDisplay, "WM_DELETE_WINDOW", False ); - XSetWMProtocols( CNFGDisplay, CNFGWindow, &WM_DELETE_WINDOW, 1 ); - XSelectInput( CNFGDisplay, CNFGWindow, KeyPressMask | KeyReleaseMask | ButtonPressMask | ButtonReleaseMask | ExposureMask | PointerMotionMask ); -} - -void CNFGHandleInput() -{ - static int ButtonsDown; - XEvent report; - - int bKeyDirection = 1; - int r; - while( XPending( CNFGDisplay ) ) - { - r=XNextEvent( CNFGDisplay, &report ); - - bKeyDirection = 1; - switch (report.type) - { - case NoExpose: - break; - case Expose: - XGetWindowAttributes( CNFGDisplay, CNFGWindow, &CNFGWinAtt ); - if( CNFGPixmap ) XFreePixmap( CNFGDisplay, CNFGPixmap ); - CNFGPixmap = XCreatePixmap( CNFGDisplay, CNFGWindow, CNFGWinAtt.width, CNFGWinAtt.height, CNFGWinAtt.depth ); - if( CNFGGC ) XFreeGC( CNFGDisplay, CNFGGC ); - CNFGGC = XCreateGC(CNFGDisplay, CNFGPixmap, 0, 0); - break; - case KeyRelease: - bKeyDirection = 0; - case KeyPress: - HandleKey( XLookupKeysym(&report.xkey, 0), bKeyDirection ); - break; - case ButtonRelease: - bKeyDirection = 0; - case ButtonPress: - HandleButton( report.xbutton.x, report.xbutton.y, report.xbutton.button, bKeyDirection ); - ButtonsDown = (ButtonsDown & (~(1<>1 ); - break; - case ClientMessage: - // Only subscribed to WM_DELETE_WINDOW, so just exit - exit( 0 ); - break; - default: - printf( "Event: %d\n", report.type ); - } - } -} - - -void CNFGUpdateScreenWithBitmap( unsigned long * data, int w, int h ) -{ - static XImage *xi; - static int depth; - static int lw, lh; - static unsigned char * lbuffer; - int r, ls; - - if( !xi ) - { - int screen = DefaultScreen(CNFGDisplay); - Visual * visual = DefaultVisual(CNFGDisplay, screen); - depth = DefaultDepth(CNFGDisplay, screen)/8; -// xi = XCreateImage(CNFGDisplay, DefaultVisual( CNFGDisplay, DefaultScreen(CNFGDisplay) ), depth*8, ZPixmap, 0, (char*)data, w, h, 32, w*4 ); -// lw = w; -// lh = h; - } - - if( lw != w || lh != h ) - { - if( xi ) free( xi ); - xi = XCreateImage(CNFGDisplay, DefaultVisual( CNFGDisplay, DefaultScreen(CNFGDisplay) ), depth*8, ZPixmap, 0, (char*)data, w, h, 32, w*4 ); - lw = w; - lh = h; - } - - ls = lw * lh; - - XPutImage(CNFGDisplay, CNFGWindow, CNFGWindowGC, xi, 0, 0, 0, 0, w, h ); -} - - -#ifndef RASTERIZER - - -uint32_t CNFGColor( uint32_t RGB ) -{ - unsigned char red = RGB & 0xFF; - unsigned char grn = ( RGB >> 8 ) & 0xFF; - unsigned char blu = ( RGB >> 16 ) & 0xFF; - CNFGLastColor = RGB; - unsigned long color = (red<<16)|(grn<<8)|(blu); - XSetForeground(CNFGDisplay, CNFGGC, color); - return color; -} - -void CNFGClearFrame() -{ - XGetWindowAttributes( CNFGDisplay, CNFGWindow, &CNFGWinAtt ); - XSetForeground(CNFGDisplay, CNFGGC, CNFGColor(CNFGBGColor) ); - XFillRectangle(CNFGDisplay, CNFGPixmap, CNFGGC, 0, 0, CNFGWinAtt.width, CNFGWinAtt.height ); -} - -void CNFGSwapBuffers() -{ - XCopyArea(CNFGDisplay, CNFGPixmap, CNFGWindow, CNFGWindowGC, 0,0,CNFGWinAtt.width,CNFGWinAtt.height,0,0); - XFlush(CNFGDisplay); - if( FullScreen ) - XSetInputFocus( CNFGDisplay, CNFGWindow, RevertToParent, CurrentTime ); -} - -void CNFGTackSegment( short x1, short y1, short x2, short y2 ) -{ - XDrawLine( CNFGDisplay, CNFGPixmap, CNFGGC, x1, y1, x2, y2 ); -} - -void CNFGTackPixel( short x1, short y1 ) -{ - XDrawPoint( CNFGDisplay, CNFGPixmap, CNFGGC, x1, y1 ); -} - -void CNFGTackRectangle( short x1, short y1, short x2, short y2 ) -{ - XFillRectangle(CNFGDisplay, CNFGPixmap, CNFGGC, x1, y1, x2-x1, y2-y1 ); -} - -void CNFGTackPoly( RDPoint * points, int verts ) -{ - XFillPolygon(CNFGDisplay, CNFGPixmap, CNFGGC, (XPoint *)points, 3, Convex, CoordModeOrigin ); -} - -#endif - diff --git a/test.c b/test.c index a7da490..7679ad4 100755 --- a/test.c +++ b/test.c @@ -5,7 +5,7 @@ #include #include -#include +#include struct SurviveContext * ctx; -- cgit v1.2.3 From 743e4b60f03db3c8c340b63b8d27eac579e146dd Mon Sep 17 00:00:00 2001 From: CNLohr Date: Fri, 17 Mar 2017 03:10:55 -0400 Subject: Update Windows build to reflect changes in RawDraw. --- calibrate.c | 4 ++++ calibrate_client.c | 3 +++ data_recorder.c | 4 ++++ winbuild/build_tcc.bat | 2 +- winbuild/libsurvive/libsurvive.vcxproj | 6 +++--- winbuild/libsurvive/libsurvive.vcxproj.filters | 18 +++++++++--------- 6 files changed, 24 insertions(+), 13 deletions(-) diff --git a/calibrate.c b/calibrate.c index 3d83baf..726a8cc 100644 --- a/calibrate.c +++ b/calibrate.c @@ -40,6 +40,10 @@ void HandleMotion( int x, int y, int mask ) { } +void HandleDestroy() +{ +} + //int bufferpts[32*2*3][2]; int bufferpts[32*2*3][2]; diff --git a/calibrate_client.c b/calibrate_client.c index 5e31cfb..1a2ee41 100644 --- a/calibrate_client.c +++ b/calibrate_client.c @@ -35,6 +35,9 @@ void HandleButton( int x, int y, int button, int bDown ) void HandleMotion( int x, int y, int mask ) { } +void HandleDestroy() +{ +} int bufferpts[32*2*3]; char buffermts[32*128*3]; diff --git a/data_recorder.c b/data_recorder.c index 630598a..afc17b2 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -33,6 +33,10 @@ void HandleMotion( int x, int y, int mask ) { } +void HandleDestroy() +{ +} + int bufferpts[32*2*3]; char buffermts[32*128*3]; int buffertimeto[32*3]; diff --git a/winbuild/build_tcc.bat b/winbuild/build_tcc.bat index 46f4beb..5be1361 100644 --- a/winbuild/build_tcc.bat +++ b/winbuild/build_tcc.bat @@ -7,7 +7,7 @@ set SR=..\src\ set RD=..\redist\ set SOURCES=%SR%ootx_decoder.c %SR%poser_charlesslow.c %SR%poser_daveortho.c %SR%poser_dummy.c %SR%survive.c %SR%survive_cal.c %SR%survive_config.c %SR%survive_data.c %SR%survive_driverman.c %SR%survive_process.c %SR%survive_vive.c set REDIST=%RD%crc32.c %RD%linmath.c %RD%puff.c %RD%jsmn.c %RD%json_helpers.c %RD%symbol_enumerator.c -set EXEC=..\calibrate.c %RD%WinDriver.c %RD%os_generic.c %RD%DrawFunctions.c +set EXEC=..\calibrate.c %RD%CNFGWinDriver.c %RD%os_generic.c %RD%CNFGFunctions.c set CFLAGS=-DNOZLIB -DTCC -DWINDOWS -DHIDAPI -DWIN32 -DRUNTIME_SYMNUM -O0 -g -rdynamic -I..\redist -I..\include\libsurvive -I..\src -I. set LDFLAGS=-lkernel32 -lgdi32 -luser32 -lsetupapi -ldbghelp @echo on diff --git a/winbuild/libsurvive/libsurvive.vcxproj b/winbuild/libsurvive/libsurvive.vcxproj index 996f342..643cff5 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj +++ b/winbuild/libsurvive/libsurvive.vcxproj @@ -139,8 +139,9 @@ + + - @@ -148,7 +149,6 @@ - @@ -166,8 +166,8 @@ + - diff --git a/winbuild/libsurvive/libsurvive.vcxproj.filters b/winbuild/libsurvive/libsurvive.vcxproj.filters index 8be02f1..0bb9d1b 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj.filters +++ b/winbuild/libsurvive/libsurvive.vcxproj.filters @@ -60,12 +60,6 @@ Source Files - - Source Files - - - Source Files - Source Files @@ -84,6 +78,12 @@ Source Files + + Source Files + + + Source Files + @@ -110,9 +110,6 @@ Header Files - - Header Files - Header Files @@ -131,5 +128,8 @@ Header Files + + Header Files + \ No newline at end of file -- cgit v1.2.3 From 4e3ac12016cc489c818709412acd293cac54cb78 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Fri, 17 Mar 2017 11:37:30 -0400 Subject: reusable function for decoding acode --- src/survive_data.c | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/src/survive_data.c b/src/survive_data.c index 4a2cfb6..d658e18 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -5,6 +5,19 @@ #include #include + +int32_t decode_acode(uint32_t length, int32_t main_divisor) { + //+50 adds a small offset and seems to help always get it right. + //Check the +50 in the future to see how well this works on a variety of hardware. + + int32_t acode = (length+main_divisor+50)/(main_divisor*2); + if( acode & 1 ) return -1; + + return (acode>>1) - 6; +} + +} + //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. void handle_lightcap( SurviveObject * so, LightcapElement * le ) { @@ -112,17 +125,13 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) int32_t acode_array[2] = { - (so->last_sync_length[0]+main_divisor+50)/(main_divisor*2), //+50 adds a small offset and seems to help always get it right. - (so->last_sync_length[1]+main_divisor+50)/(main_divisor*2), //Check the +50 in the future to see how well this works on a variety of hardware. + decode_acode(so->last_sync_length[0],main_divisor), + decode_acode(so->last_sync_length[1],main_divisor) }; //XXX: TODO: Capture error count here. - if( acode_array[0] & 1 ) return; - if( acode_array[1] & 1 ) return; - - acode_array[0] = (acode_array[0]>>1) - 6; - acode_array[1] = (acode_array[1]>>1) - 6; - + if( acode_array[0] < 0 ) return; + if( acode_array[1] < 0 ) return; int acode = acode_array[0]; -- cgit v1.2.3 From c90a23cc928b7434a1e3aceb36b783761bfc8d7e Mon Sep 17 00:00:00 2001 From: geekmaster Date: Fri, 17 Mar 2017 13:13:12 -0500 Subject: Update README.md Add Intel Integrated Graphics section. --- README.md | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 6e097f8..45ac38f 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # libsurvive -**WARNING PROJECT NOT YET IN EXPERIMENTAL PHASE** +**WARNING PROJECT IN EXPERIMENTAL PHASE** Discord: https://discordapp.com/invite/7QbCAGS @@ -40,8 +40,7 @@ I say "high-performance" really this project is based tightly off of OSVR-Vive-L 2. Put it under an open-source instead of a force-source license. (GPL to MIT/X11) 3. Write it in C. 4. Avoid extra layers where convenient. -5. (long shot) Make the vive vivable for use with Intel Integrated Graphics systems. - +5. (long shot) Make the vive vivable for use with Intel Integrated Graphics systems. [It works with HD4000 using DisplayPort. See "Intel Integrated Graphics" section below.] Will ~~I~~ we succeed? Probably not. @@ -50,7 +49,7 @@ Definitely going to try! ## External dependencies -* libUSB +* libUSB (hidapi ???) * pthread * libX11 (where applicable) * zlib (may use puff.c if needed) @@ -71,7 +70,10 @@ It is written in some fairly stout "layers" which are basically just function ca I may or may not read data from the Vive regarding configuration. If I do, it would be added to the survive_usb.c +## Intel Integrated Graphics +The limiting factor for Vive viability on a given computer is the maximum available pixel clock frequency, and frequency limitations of the HDMI port, and HDMI and DisplayPort video cables. DisplayPort can support higher frequencies than HDMI, on Ivy Bridge HD4000 graphics. In fact, the vive works with HD4000 graphics using DisplayPort, with native EDID resolution (2160x1200@90Hz). +To support the Vive on HDMI, you either need a newer version of HDMI, or you need to define a custom resolution that respects pixel clock and video port limits, and is also accepted and displayed by the Vive. So far, we have not had success using custom resolutions on linux or on Windows. Windows imposes additional limitations in the form of restriction of WHQL certified drivers forbidden from using custom display resolutions (only allowing those defined by EDID in the monitor). Intel has released uncertified beta drivers for Haswell and newer processors, which should be able to support custom resolutions for the Vive (untested at this time). - +but HDMI will require non-certified drivers to allow custom resolutions (due to WHQL restrictions). Haswell (Iris) graphics and later can use the new intel beta drivers to allow custom resolutions.] -- cgit v1.2.3 From 94be8ccdbfb2f44c9bc569428537444030ba8eeb Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 17 Mar 2017 13:46:34 -0700 Subject: Wired Watchman Now Emit Light & IMU Data --- data_recorder.c | 2 + src/survive_vive.c | 101 +++++++++++++++++++++++++++++++++++++++++------- test.c | 3 ++ winbuild/libsurvive.sln | 20 ++++++++++ 4 files changed, 111 insertions(+), 15 deletions(-) diff --git a/data_recorder.c b/data_recorder.c index 5504d42..206c6c3 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -1,6 +1,8 @@ //Data recorder mod with GUI showing light positions. +#ifdef __linux__ #include +#endif #include #include #include diff --git a/src/survive_vive.c b/src/survive_vive.c index 1929b1a..d4a853d 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -39,13 +39,15 @@ struct SurviveViveData; const short vidpids[] = { - 0x0bb4, 0x2c87, 0, //The main HTC HMD device - 0x28de, 0x2000, 0, //Valve lighthouse + 0x0bb4, 0x2c87, 0, //Valve HMD Button and face proximity sensor + 0x28de, 0x2000, 0, //Valve HMD IMU & Lighthouse Sensors 0x28de, 0x2101, 0, //Valve Watchman 0x28de, 0x2101, 1, //Valve Watchman 0x28de, 0x2022, 0, //HTC Tracker + 0x28de, 0x2012, 0, //Valve Watchman, USB connected #ifdef HIDAPI - 0x28de, 0x2000, 1, //Valve lighthouse(B) (only used on HIDAPI, for lightcap) + 0x28de, 0x2000, 1, //Valve HMD lighthouse(B) (only used on HIDAPI, for lightcap) + 0x28de, 0x2012, 1, //Valve Watchman, USB connected (only used on HIDAPI, for lightcap) #endif }; //length MAX_USB_INTERFACES*2 @@ -55,8 +57,10 @@ const char * devnames[] = { "Watchman 1", "Watchman 2", "Tracker 0", + "Wired Watchman 1", #ifdef HIDAPI - "Lightcap", + "HMD Lightcap", + "Wired Watchman 1 Lightcap", #endif }; //length MAX_USB_INTERFACES @@ -66,12 +70,14 @@ const char * devnames[] = { #define USB_DEV_WATCHMAN1 2 #define USB_DEV_WATCHMAN2 3 #define USB_DEV_TRACKER0 4 +#define USB_DEV_W_WATCHMAN1 5 // Wired Watchman attached via USB #ifdef HIDAPI -#define USB_DEV_LIGHTHOUSEB 5 -#define MAX_USB_DEVS 6 +#define USB_DEV_LIGHTHOUSEB 6 +#define USB_DEV_W_WATCHMAN1_LIGHTCAP 7 +#define MAX_USB_DEVS 8 #else -#define MAX_USB_DEVS 5 +#define MAX_USB_DEVS 6 #endif #define USB_IF_HMD 0 @@ -79,8 +85,10 @@ const char * devnames[] = { #define USB_IF_WATCHMAN1 2 #define USB_IF_WATCHMAN2 3 #define USB_IF_TRACKER0 4 -#define USB_IF_LIGHTCAP 5 -#define MAX_INTERFACES 6 +#define USB_IF_W_WATCHMAN1 5 +#define USB_IF_LIGHTCAP 6 +#define USB_IF_W_WATCHMAN1_LIGHTCAP 7 +#define MAX_INTERFACES 8 typedef struct SurviveUSBInterface SurviveUSBInterface; typedef struct SurviveViveData SurviveViveData; @@ -299,7 +307,7 @@ static inline int hid_get_feature_report_timeout(USBHANDLE device, uint16_t ifac return -1; } -int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, struct SurviveObject *wm0, struct SurviveObject * wm1, struct SurviveObject * tr0 ) +int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, struct SurviveObject *wm0, struct SurviveObject * wm1, struct SurviveObject * tr0 , struct SurviveObject * ww0 ) { struct SurviveContext * ctx = sv->ctx; #ifdef HIDAPI @@ -463,11 +471,14 @@ int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, s if( sv->udev[USB_DEV_WATCHMAN1] && AttachInterface( sv, wm0, USB_IF_WATCHMAN1, sv->udev[USB_DEV_WATCHMAN1], 0x81, survive_data_cb, "Watchman 1" ) ) { return -8; } if( sv->udev[USB_DEV_WATCHMAN2] && AttachInterface( sv, wm1, USB_IF_WATCHMAN2, sv->udev[USB_DEV_WATCHMAN2], 0x81, survive_data_cb, "Watchman 2")) { return -9; } if( sv->udev[USB_DEV_TRACKER0] && AttachInterface( sv, tr0, USB_IF_TRACKER0, sv->udev[USB_DEV_TRACKER0], 0x81, survive_data_cb, "Tracker 1")) { return -10; } + if( sv->udev[USB_DEV_W_WATCHMAN1] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1, sv->udev[USB_DEV_W_WATCHMAN1], 0x81, survive_data_cb, "Wired Watchman 1")) { return -11; } #ifdef HIDAPI //Tricky: use other interface for actual lightcap. XXX THIS IS NOT YET RIGHT!!! if( sv->udev[USB_DEV_LIGHTHOUSEB] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_LIGHTHOUSEB], 0x82, survive_data_cb, "Lightcap")) { return -12; } + if( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; } #else if( sv->udev[USB_DEV_LIGHTHOUSE] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_LIGHTHOUSE], 0x82, survive_data_cb, "Lightcap")) { return -12; } + if( sv->udev[USB_DEV_W_WATCHMAN1] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; } #endif SV_INFO( "All enumerated devices attached." ); @@ -508,8 +519,19 @@ int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_c if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; } + if (sv->udev[USB_DEV_W_WATCHMAN1]) + { + static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; + r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); + if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; + + static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates) + r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); + if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; + } + #if 0 - for( i = 0; i < 256; i++ ) + for( int i = 0; i < 256; i++ ) { static uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0xff, 0, 0, 0, 0, 0, 0, 0 }; r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); @@ -518,6 +540,14 @@ int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_c OGUSleep( 1000 ); } #endif + + if (sv->udev[USB_DEV_TRACKER0]) + { + static uint8_t vive_magic_power_on[64] = { 0x04, 0x78, 0x29, 0x38 }; + r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); + if( r != sizeof( vive_magic_power_on ) ) return 5; + } + SV_INFO( "Powered unit on." ); } else @@ -550,7 +580,7 @@ int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_c if( r != sizeof( vive_magic_power_off2 ) ) return 5; } } - + return 0; } void survive_vive_usb_close( struct SurviveViveData * sv ) @@ -584,6 +614,7 @@ int survive_vive_usb_poll( struct SurviveContext * ctx, void * v ) OGUnlockMutex( GlobalRXUSBMutx ); OGUSleep( 100 ); OGUnlockMutex( GlobalRXUSBMutx ); + return 0; #else struct SurviveViveData * sv = v; int r = libusb_handle_events( sv->usbctx ); @@ -642,6 +673,7 @@ int survive_get_config( char ** config, struct SurviveViveData * sv, int devno, OGUSleep(1000); } + // Send Report 16 to prepare the device for reading config info memset( cfgbuff, 0, sizeof( cfgbuff ) ); cfgbuff[0] = 0x10; if( ( ret = hid_get_feature_report_timeout( dev, iface, cfgbuff, sizeof( cfgbuff ) ) ) < 0 ) @@ -651,6 +683,7 @@ int survive_get_config( char ** config, struct SurviveViveData * sv, int devno, } + // Now do a bunch of Report 17 until there are no bytes left cfgbuff[1] = 0xaa; cfgbuff[0] = 0x11; do @@ -996,6 +1029,7 @@ void survive_data_cb( SurviveUSBInterface * si ) break; } case USB_IF_LIGHTHOUSE: + case USB_IF_W_WATCHMAN1: { int i; //printf( "%d -> ", size ); @@ -1055,7 +1089,7 @@ void survive_data_cb( SurviveUSBInterface * si ) // TODO: Looks like this will need to be handle_tracker, since // it appears the interface is sufficiently different. // More work needd to reverse engineer it. - handle_watchman( w, readdata); + //handle_wired_watchman( w, readdata, size); } else { @@ -1077,6 +1111,20 @@ void survive_data_cb( SurviveUSBInterface * si ) } break; } + case USB_IF_W_WATCHMAN1_LIGHTCAP: + { + int i; + for( i = 0; i < 7; i++ ) + { + LightcapElement le; + le.sensor_id = POP2; + le.length = POP2; + le.timestamp = POP4; + if( le.sensor_id == 0xff ) break; + handle_lightcap( obj, &le ); + } + break; + } } } @@ -1234,6 +1282,8 @@ int survive_vive_close( SurviveContext * ctx, void * driver ) SurviveViveData * sv = driver; survive_vive_usb_close( sv ); + + return 0; } @@ -1244,6 +1294,7 @@ int DriverRegHTCVive( SurviveContext * ctx ) SurviveObject * wm0 = calloc( 1, sizeof( SurviveObject ) ); SurviveObject * wm1 = calloc( 1, sizeof( SurviveObject ) ); SurviveObject * tr0 = calloc( 1, sizeof( SurviveObject ) ); + SurviveObject * ww0 = calloc( 1, sizeof( SurviveObject ) ); SurviveViveData * sv = calloc( 1, sizeof( SurviveViveData ) ); sv->ctx = ctx; @@ -1269,9 +1320,12 @@ int DriverRegHTCVive( SurviveContext * ctx ) tr0->ctx = ctx; memcpy( tr0->codename, "TR0", 4 ); memcpy( tr0->drivername, "HTC", 4 ); + ww0->ctx = ctx; + memcpy( ww0->codename, "WW0", 4 ); + memcpy( ww0->drivername, "HTC", 4 ); //USB must happen last. - if( r = survive_usb_init( sv, hmd, wm0, wm1, tr0) ) + if( r = survive_usb_init( sv, hmd, wm0, wm1, tr0, ww0) ) { //TODO: Cleanup any libUSB stuff sitting around. goto fail_gracefully; @@ -1281,20 +1335,35 @@ int DriverRegHTCVive( SurviveContext * ctx ) if( sv->udev[USB_DEV_HMD] && LoadConfig( sv, hmd, 1, 0, 0 )) { SV_INFO( "HMD config issue." ); } if( sv->udev[USB_DEV_WATCHMAN1] && LoadConfig( sv, wm0, 2, 0, 1 )) { SV_INFO( "Watchman 0 config issue." ); } if( sv->udev[USB_DEV_WATCHMAN2] && LoadConfig( sv, wm1, 3, 0, 1 )) { SV_INFO( "Watchman 1 config issue." ); } - if( sv->udev[USB_DEV_TRACKER0] && LoadConfig( sv, tr0, 4, 0, 1 )) { SV_INFO( "Tracker 0 config issue." ); } + if( sv->udev[USB_DEV_TRACKER0] && LoadConfig( sv, tr0, 4, 0, 0 )) { SV_INFO( "Tracker 0 config issue." ); } + if( sv->udev[USB_DEV_W_WATCHMAN1] && LoadConfig( sv, ww0, 5, 0, 0 )) { SV_INFO( "Wired Watchman 0 config issue." ); } hmd->timebase_hz = wm0->timebase_hz = wm1->timebase_hz = 48000000; + tr0->timebase_hz = ww0->timebase_hz = hmd->timebase_hz; + hmd->pulsedist_max_ticks = wm0->pulsedist_max_ticks = wm1->pulsedist_max_ticks = 500000; + tr0->pulsedist_max_ticks = ww0->pulsedist_max_ticks = hmd->pulsedist_max_ticks; + hmd->pulselength_min_sync = wm0->pulselength_min_sync = wm1->pulselength_min_sync = 2200; + tr0->pulselength_min_sync = ww0->pulselength_min_sync = hmd->pulselength_min_sync; + hmd->pulse_in_clear_time = wm0->pulse_in_clear_time = wm1->pulse_in_clear_time = 35000; + tr0->pulse_in_clear_time = ww0->pulse_in_clear_time = hmd->pulse_in_clear_time; + hmd->pulse_max_for_sweep = wm0->pulse_max_for_sweep = wm1->pulse_max_for_sweep = 1800; + tr0->pulse_max_for_sweep = ww0->pulse_max_for_sweep = hmd->pulse_max_for_sweep; hmd->pulse_synctime_offset = wm0->pulse_synctime_offset = wm1->pulse_synctime_offset = 20000; + tr0->pulse_synctime_offset = ww0->pulse_synctime_offset = hmd->pulse_synctime_offset; + hmd->pulse_synctime_slack = wm0->pulse_synctime_slack = wm1->pulse_synctime_slack = 5000; + tr0->pulse_synctime_slack = ww0->pulse_synctime_slack = hmd->pulse_synctime_slack; hmd->timecenter_ticks = hmd->timebase_hz / 240; wm0->timecenter_ticks = wm0->timebase_hz / 240; wm1->timecenter_ticks = wm1->timebase_hz / 240; + tr0->timecenter_ticks = tr0->timebase_hz / 240; + ww0->timecenter_ticks = ww0->timebase_hz / 240; /* int i; int locs = hmd->nr_locations; @@ -1322,6 +1391,7 @@ int DriverRegHTCVive( SurviveContext * ctx ) if( sv->udev[USB_DEV_WATCHMAN1] ) { survive_add_object( ctx, wm0 ); } if( sv->udev[USB_DEV_WATCHMAN2] ) { survive_add_object( ctx, wm1 ); } if( sv->udev[USB_DEV_TRACKER0] ) { survive_add_object( ctx, tr0 ); } + if( sv->udev[USB_DEV_W_WATCHMAN1] ) { survive_add_object( ctx, ww0 ); } survive_add_driver( ctx, sv, survive_vive_usb_poll, survive_vive_close, survive_vive_send_magic ); @@ -1331,6 +1401,7 @@ fail_gracefully: free( wm0 ); free( wm1 ); free( tr0 ); + free( ww0 ); survive_vive_usb_close( sv ); free( sv ); return -1; diff --git a/test.c b/test.c index a7da490..e34a7a8 100755 --- a/test.c +++ b/test.c @@ -1,4 +1,6 @@ +#ifdef __linux__ #include +#endif #include #include #include @@ -56,6 +58,7 @@ int main() dump_iface( survive_get_so_by_name( ctx, "WM0" ), "WM0" ); dump_iface( survive_get_so_by_name( ctx, "WM1" ), "WM1" ); dump_iface( survive_get_so_by_name( ctx, "TR0" ), "TR0" ); + dump_iface( survive_get_so_by_name( ctx, "WW0" ), "WW0" ); while(survive_poll(ctx) == 0) { diff --git a/winbuild/libsurvive.sln b/winbuild/libsurvive.sln index 8924631..b525975 100644 --- a/winbuild/libsurvive.sln +++ b/winbuild/libsurvive.sln @@ -7,6 +7,10 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "calibrate", "calibrate\cali EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "libsurvive", "libsurvive\libsurvive.vcxproj", "{435CFD2C-8724-42EE-8FDE-71EF7D2C6B2F}" EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "data_recorder", "data_recorder\data_recorder.vcxproj", "{265C4E2C-66CB-4954-97CA-194D69B5A673}" +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "test", "test\test.vcxproj", "{EF083B79-F1D7-408A-9902-502B9A0639E0}" +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|x64 = Debug|x64 @@ -31,6 +35,22 @@ Global {435CFD2C-8724-42EE-8FDE-71EF7D2C6B2F}.Release|x64.Build.0 = Release|x64 {435CFD2C-8724-42EE-8FDE-71EF7D2C6B2F}.Release|x86.ActiveCfg = Release|Win32 {435CFD2C-8724-42EE-8FDE-71EF7D2C6B2F}.Release|x86.Build.0 = Release|Win32 + {265C4E2C-66CB-4954-97CA-194D69B5A673}.Debug|x64.ActiveCfg = Debug|x64 + {265C4E2C-66CB-4954-97CA-194D69B5A673}.Debug|x64.Build.0 = Debug|x64 + {265C4E2C-66CB-4954-97CA-194D69B5A673}.Debug|x86.ActiveCfg = Debug|Win32 + {265C4E2C-66CB-4954-97CA-194D69B5A673}.Debug|x86.Build.0 = Debug|Win32 + {265C4E2C-66CB-4954-97CA-194D69B5A673}.Release|x64.ActiveCfg = Release|x64 + {265C4E2C-66CB-4954-97CA-194D69B5A673}.Release|x64.Build.0 = Release|x64 + {265C4E2C-66CB-4954-97CA-194D69B5A673}.Release|x86.ActiveCfg = Release|Win32 + {265C4E2C-66CB-4954-97CA-194D69B5A673}.Release|x86.Build.0 = Release|Win32 + {EF083B79-F1D7-408A-9902-502B9A0639E0}.Debug|x64.ActiveCfg = Debug|x64 + {EF083B79-F1D7-408A-9902-502B9A0639E0}.Debug|x64.Build.0 = Debug|x64 + {EF083B79-F1D7-408A-9902-502B9A0639E0}.Debug|x86.ActiveCfg = Debug|Win32 + {EF083B79-F1D7-408A-9902-502B9A0639E0}.Debug|x86.Build.0 = Debug|Win32 + {EF083B79-F1D7-408A-9902-502B9A0639E0}.Release|x64.ActiveCfg = Release|x64 + {EF083B79-F1D7-408A-9902-502B9A0639E0}.Release|x64.Build.0 = Release|x64 + {EF083B79-F1D7-408A-9902-502B9A0639E0}.Release|x86.ActiveCfg = Release|Win32 + {EF083B79-F1D7-408A-9902-502B9A0639E0}.Release|x86.Build.0 = Release|Win32 EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE -- cgit v1.2.3 From e595429d66e67b12f203cd6cd813049adcbd7cb3 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 17 Mar 2017 14:44:50 -0700 Subject: Add data recorder --- winbuild/data_recorder/data_recorder.vcxproj | 178 +++++++++++++++++++++ .../data_recorder/data_recorder.vcxproj.filters | 22 +++ 2 files changed, 200 insertions(+) create mode 100644 winbuild/data_recorder/data_recorder.vcxproj create mode 100644 winbuild/data_recorder/data_recorder.vcxproj.filters diff --git a/winbuild/data_recorder/data_recorder.vcxproj b/winbuild/data_recorder/data_recorder.vcxproj new file mode 100644 index 0000000..59a1e77 --- /dev/null +++ b/winbuild/data_recorder/data_recorder.vcxproj @@ -0,0 +1,178 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 15.0 + {265C4E2C-66CB-4954-97CA-194D69B5A673} + Win32Proj + data_recorder + 10.0.14393.0 + + + + Application + true + v141 + MultiByte + + + Application + false + v141 + true + MultiByte + + + Application + true + v141 + MultiByte + + + Application + false + v141 + true + MultiByte + + + + + + + + + + + + + + + + + + + + + true + + + true + + + false + + + false + + + + + + Level3 + Disabled + USE_DOUBLE;RUNTIME_SYMNUMX;HIDAPI;_CRT_SECURE_NO_WARNINGS;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + ..\..\windows;..\..\include\libsurvive;..\..\redist; + + + Console + setupapi.lib;dbghelp.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + true + UseFastLinkTimeCodeGeneration + + + true + + + + + + + Level3 + Disabled + USE_DOUBLE;HIDAPI;_CRT_SECURE_NO_WARNINGS;WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + ..\..\windows;..\..\include\libsurvive;..\..\redist; + + + Console + setupapi.lib;dbghelp.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + true + UseFastLinkTimeCodeGeneration + + + true + + + + + Level3 + + + MaxSpeed + true + true + USE_DOUBLE;HIDAPI;_CRT_SECURE_NO_WARNINGS;WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + ..\..\windows;..\..\include\libsurvive;..\..\redist; + + + Console + true + true + setupapi.lib;dbghelp.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + true + + + true + + + + + Level3 + + + MaxSpeed + true + true + USE_DOUBLE;HIDAPI;_CRT_SECURE_NO_WARNINGS;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + ..\..\windows;..\..\include\libsurvive;..\..\redist; + + + Console + true + true + setupapi.lib;dbghelp.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + true + + + true + + + + + {435cfd2c-8724-42ee-8fde-71ef7d2c6b2f} + + + + + + + + + \ No newline at end of file diff --git a/winbuild/data_recorder/data_recorder.vcxproj.filters b/winbuild/data_recorder/data_recorder.vcxproj.filters new file mode 100644 index 0000000..c696f0f --- /dev/null +++ b/winbuild/data_recorder/data_recorder.vcxproj.filters @@ -0,0 +1,22 @@ + + + + + {4FC737F1-C7A5-4376-A066-2A32D752A2FF} + cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx + + + {93995380-89BD-4b04-88EB-625FBE52EBFB} + h;hh;hpp;hxx;hm;inl;inc;xsd + + + {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} + rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms + + + + + Source Files + + + \ No newline at end of file -- cgit v1.2.3 From 2a499b80e14596b72fde148b79fed77fd35de507 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 17 Mar 2017 14:45:42 -0700 Subject: Add Test Project --- winbuild/test/test.vcxproj | 178 +++++++++++++++++++++++++++++++++++++ winbuild/test/test.vcxproj.filters | 22 +++++ 2 files changed, 200 insertions(+) create mode 100644 winbuild/test/test.vcxproj create mode 100644 winbuild/test/test.vcxproj.filters diff --git a/winbuild/test/test.vcxproj b/winbuild/test/test.vcxproj new file mode 100644 index 0000000..e6ee3fb --- /dev/null +++ b/winbuild/test/test.vcxproj @@ -0,0 +1,178 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 15.0 + {EF083B79-F1D7-408A-9902-502B9A0639E0} + Win32Proj + test + 10.0.14393.0 + + + + Application + true + v141 + Unicode + + + Application + false + v141 + true + Unicode + + + Application + true + v141 + Unicode + + + Application + false + v141 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + true + + + false + + + false + + + + + + Level3 + Disabled + USE_DOUBLE;RUNTIME_SYMNUMX;HIDAPI;_CRT_SECURE_NO_WARNINGS;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + ..\..\windows;..\..\include\libsurvive;..\..\redist; + + + Console + setupapi.lib;dbghelp.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + true + UseFastLinkTimeCodeGeneration + + + true + + + + + + + Level3 + Disabled + USE_DOUBLE;HIDAPI;_CRT_SECURE_NO_WARNINGS;WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + ..\..\windows;..\..\include\libsurvive;..\..\redist; + + + Console + setupapi.lib;dbghelp.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + true + UseFastLinkTimeCodeGeneration + + + true + + + + + Level3 + + + MaxSpeed + true + true + USE_DOUBLE;HIDAPI;_CRT_SECURE_NO_WARNINGS;WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + ..\..\windows;..\..\include\libsurvive;..\..\redist; + + + Console + true + true + setupapi.lib;dbghelp.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + true + + + true + + + + + Level3 + + + MaxSpeed + true + true + USE_DOUBLE;HIDAPI;_CRT_SECURE_NO_WARNINGS;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + ..\..\windows;..\..\include\libsurvive;..\..\redist; + + + Console + true + true + setupapi.lib;dbghelp.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + true + + + true + + + + + {435cfd2c-8724-42ee-8fde-71ef7d2c6b2f} + + + + + + + + + \ No newline at end of file diff --git a/winbuild/test/test.vcxproj.filters b/winbuild/test/test.vcxproj.filters new file mode 100644 index 0000000..60c3446 --- /dev/null +++ b/winbuild/test/test.vcxproj.filters @@ -0,0 +1,22 @@ + + + + + {4FC737F1-C7A5-4376-A066-2A32D752A2FF} + cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx + + + {93995380-89BD-4b04-88EB-625FBE52EBFB} + h;hh;hpp;hxx;hm;inl;inc;xsd + + + {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} + rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms + + + + + Source Files + + + \ No newline at end of file -- cgit v1.2.3 From 64e3b67aa0d6a970e1e1988881e77d04c4fb5b07 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 17 Mar 2017 14:51:00 -0700 Subject: Remove extra debug print --- src/survive_data.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/survive_data.c b/src/survive_data.c index b9099ac..5c5a5e9 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -39,7 +39,6 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) // return; //if we don't know what lighthouse this is we don't care to do much else } - printf("m sync %d %d %d %d\n", le->sensor_id, so->last_sync_time[ssn], le->timestamp, delta); if( le->length > so->pulselength_min_sync ) //Pulse longer indicates a sync pulse. { -- cgit v1.2.3 From e1818d84e1ab0ae217d8035f8a12398f06508059 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 17 Mar 2017 16:32:18 -0700 Subject: Alternate disambiguator and calibration updates --- src/survive_cal.c | 50 ++++++++++++++++++++++++++-------- src/survive_cal.h | 6 +++-- src/survive_data.c | 78 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 121 insertions(+), 13 deletions(-) diff --git a/src/survive_cal.c b/src/survive_cal.c index 0eb9446..c36a48a 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -30,6 +30,11 @@ int mkdir(const char *); #define DRPTS_NEEDED_FOR_AVG ((int)(DRPTS*3/4)) + //at stage 1, is when it branches to stage two or stage 7 + //stage 2 checks for the presence of two watchmen and an HMD visible to both lighthouses. + //Stage 3 collects a bunch of data for statistical averageing + //stage 4 does the calculation for the poses (NOT DONE!) + //Stage 5 = System Calibrate.d static void handle_calibration( struct SurviveCalData *cd ); @@ -117,17 +122,36 @@ void survive_cal_install( struct SurviveContext * ctx ) cd->stage = 1; cd->ctx = ctx; - cd->poseobjects[0] = survive_get_so_by_name( ctx, "HMD" ); - cd->poseobjects[1] = survive_get_so_by_name( ctx, "WM0" ); - cd->poseobjects[2] = survive_get_so_by_name( ctx, "WM1" ); + cd->numPoseObjects = 0; - if( cd->poseobjects[0] == 0 || cd->poseobjects[1] == 0 || cd->poseobjects[2] == 0 ) + for (int i=0; i < ctx->objs_ct; i++) { - SV_ERROR( "Error: cannot find all devices needed for calibration." ); - free( cd ); - return; + // This would be a place where we could conditionally decide if we + // want to include a certain device in the calibration routine. + if (1) + { + cd->poseobjects[i] = ctx->objs[i]; + cd->numPoseObjects++; + + SV_INFO("Calibration is using %s", cd->poseobjects[i]->codename); + } } + // If we want to mandate that certain devices have been found + + + + //cd->poseobjects[0] = survive_get_so_by_name( ctx, "HMD" ); + //cd->poseobjects[1] = survive_get_so_by_name( ctx, "WM0" ); + //cd->poseobjects[2] = survive_get_so_by_name( ctx, "WM1" ); + + //if( cd->poseobjects[0] == 0 || cd->poseobjects[1] == 0 || cd->poseobjects[2] == 0 ) + //{ + // SV_ERROR( "Error: cannot find all devices needed for calibration." ); + // free( cd ); + // return; + //} + //XXX TODO MWTourney, work on your code here. /* if( !cd->hmd ) @@ -185,7 +209,9 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int if( sensor_id < 0 ) { int lhid = -sensor_id-1; - if( lhid < NUM_LIGHTHOUSES && so->codename[0] == 'H' ) + // Take the OOTX data from the first device. + // TODO: Improve this so we'll watch all devices looking for OOTX data. Why limit ourselves to just one? + if( lhid < NUM_LIGHTHOUSES && so == cd->poseobjects[0] ) { uint8_t dbit = (acode & 2)>>1; ootx_pump_bit( &cd->ootx_decoders[lhid], dbit ); @@ -193,7 +219,9 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int int i; for( i = 0; i < NUM_LIGHTHOUSES; i++ ) if( ctx->bsd[i].OOTXSet == 0 ) break; - if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //If all lighthouses have their OOTX set, move on. + //if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //If all lighthouses have their OOTX set, move on. <------- Revert This!!!!! + if( i == 1 ) + cd->stage = 2; //If all lighthouses have their OOTX set, move on. } break; } @@ -498,11 +526,11 @@ static void handle_calibration( struct SurviveCalData *cd ) int obj; //Poses of lighthouses relative to objects. - SurvivePose objphl[POSE_OBJECTS][NUM_LIGHTHOUSES]; + SurvivePose objphl[MAX_POSE_OBJECTS][NUM_LIGHTHOUSES]; FILE * fobjp = fopen( "calinfo/objposes.csv", "w" ); - for( obj = 0; obj < POSE_OBJECTS; obj++ ) + for( obj = 0; obj < cd->numPoseObjects; obj++ ) { int i, j; PoserDataFullScene fsd; diff --git a/src/survive_cal.h b/src/survive_cal.h index 3d62051..8c488bd 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -37,7 +37,7 @@ void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t t #define MIN_PTS_BEFORE_CAL 24 #define DRPTS 128 -#define POSE_OBJECTS 3 +#define MAX_POSE_OBJECTS 10 #define MAX_CAL_PT_DAT (MAX_SENSORS_TO_CAL*NUM_LIGHTHOUSES*2) struct SurviveCalData @@ -64,7 +64,9 @@ struct SurviveCalData int senid_of_checkpt; //This is a point on a watchman that can be used to check the lh solution. - SurviveObject * poseobjects[POSE_OBJECTS]; + SurviveObject * poseobjects[MAX_POSE_OBJECTS]; + + size_t numPoseObjects; PoserCB ConfigPoserFn; diff --git a/src/survive_data.c b/src/survive_data.c index 5c5a5e9..e6538d1 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -5,9 +5,87 @@ #include #include +int getAcodeFromSyncPulse(int pulseLen) +{ + if (pulseLen < 3125) return 0; + if (pulseLen < 3625) return 1; + if (pulseLen < 4125) return 2; + if (pulseLen < 4625) return 3; + if (pulseLen < 5125) return 4; + if (pulseLen < 5625) return 5; + if (pulseLen < 6125) return 6; + return 7; +} +void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) +{ + fprintf(stderr, "%d\n", le->length); + + if (le->timestamp - so->recent_sync_time < 24000) + { + // I do believe we are lighthouse B + so->last_sync_time[1] = so->recent_sync_time = le->timestamp; + so->last_sync_length[1] = le->length; + if (le->length < 4625) // max non-skip pulse + 1/4 the difference in time between pulses. + { + so->sync_set_number = 1; + so->ctx->lightproc(so, -2, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right. + } + } + else + { + // looks like this is the first sync pulse. Cool! + if (le->length < 4625) // max non-skip pulse + 1/4 the difference in time between pulses. + { + so->sync_set_number = 1; + so->ctx->lightproc(so, -1, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right. + } + } + + +// ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1] ); + + +// typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); + +} + +void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) +{ + +} + +void handle_lightcap2( SurviveObject * so, LightcapElement * le ) +{ + SurviveContext * ctx = so->ctx; + + if( le->sensor_id > SENSORS_PER_OBJECT ) + { + return; + } + + if (le->length > 6750) + { + // Should never get a reading so high. Odd. + return; + } + if (le->length >= 2750) + { + // Looks like a sync pulse, process it! + handle_lightcap2_sync(so, le); + } + + // must be a sweep pulse, process it! + handle_lightcap2_sweep(so, le); + +} + + //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. void handle_lightcap( SurviveObject * so, LightcapElement * le ) { + handle_lightcap2(so,le); + return; + SurviveContext * ctx = so->ctx; //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; -- cgit v1.2.3 From acd7b7c0ecc7482ca60cdefeb277543212a293f5 Mon Sep 17 00:00:00 2001 From: geekmaster Date: Fri, 17 Mar 2017 18:34:30 -0500 Subject: Update README.md Fix typos. --- README.md | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 45ac38f..fcde197 100644 --- a/README.md +++ b/README.md @@ -40,7 +40,7 @@ I say "high-performance" really this project is based tightly off of OSVR-Vive-L 2. Put it under an open-source instead of a force-source license. (GPL to MIT/X11) 3. Write it in C. 4. Avoid extra layers where convenient. -5. (long shot) Make the vive vivable for use with Intel Integrated Graphics systems. [It works with HD4000 using DisplayPort. See "Intel Integrated Graphics" section below.] +5. (long shot) Make the vive viable for use with Intel Integrated Graphics systems. [It works with HD4000 using DisplayPort. See "Intel Integrated Graphics" section below.] Will ~~I~~ we succeed? Probably not. @@ -72,8 +72,6 @@ I may or may not read data from the Vive regarding configuration. If I do, it w ## Intel Integrated Graphics -The limiting factor for Vive viability on a given computer is the maximum available pixel clock frequency, and frequency limitations of the HDMI port, and HDMI and DisplayPort video cables. DisplayPort can support higher frequencies than HDMI, on Ivy Bridge HD4000 graphics. In fact, the vive works with HD4000 graphics using DisplayPort, with native EDID resolution (2160x1200@90Hz). +The limiting factor for Vive viability on a given computer is the maximum available pixel clock frequency, and frequency limitations of the HDMI port, and HDMI and DisplayPort video cables. DisplayPort can support higher frequencies than HDMI, on Ivy Bridge HD4000 graphics. In fact, the Vive works with HD4000 graphics using DisplayPort, with native EDID resolution (2160x1200@90Hz). To support the Vive on HDMI, you either need a newer version of HDMI, or you need to define a custom resolution that respects pixel clock and video port limits, and is also accepted and displayed by the Vive. So far, we have not had success using custom resolutions on linux or on Windows. Windows imposes additional limitations in the form of restriction of WHQL certified drivers forbidden from using custom display resolutions (only allowing those defined by EDID in the monitor). Intel has released uncertified beta drivers for Haswell and newer processors, which should be able to support custom resolutions for the Vive (untested at this time). - -but HDMI will require non-certified drivers to allow custom resolutions (due to WHQL restrictions). Haswell (Iris) graphics and later can use the new intel beta drivers to allow custom resolutions.] -- cgit v1.2.3 From 129677f041dc87689e73d85ef737501be59b58ca Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 18 Mar 2017 01:01:49 -0400 Subject: Fix mixed usage of "struct" and typedef'd struct, and fix compile error on gcc/Linux. --- src/survive_vive.c | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/src/survive_vive.c b/src/survive_vive.c index a9b295f..91efc2c 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -141,10 +141,10 @@ void survive_data_cb( SurviveUSBInterface * si ); //USB Subsystem void survive_usb_close( SurviveContext * t ); -int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1, SurviveObject * tr0 ); +int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1, SurviveObject * tr0 , SurviveObject * ww0 ); int survive_usb_poll( SurviveContext * ctx ); int survive_get_config( char ** config, SurviveViveData * ctx, int devno, int iface, int send_extra_magic ); -int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen ); +int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen ); #ifdef HIDAPI void * HAPIReceiver( void * v ) @@ -177,8 +177,8 @@ void * HAPIReceiver( void * v ) #else static void handle_transfer(struct libusb_transfer* transfer) { - struct SurviveUSBInterface * iface = transfer->user_data; - struct SurviveContext * ctx = iface->ctx; + SurviveUSBInterface * iface = transfer->user_data; + SurviveContext * ctx = iface->ctx; if( transfer->status != LIBUSB_TRANSFER_COMPLETED ) { @@ -307,9 +307,9 @@ static inline int hid_get_feature_report_timeout(USBHANDLE device, uint16_t ifac return -1; } -int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, struct SurviveObject *wm0, struct SurviveObject * wm1, struct SurviveObject * tr0 , struct SurviveObject * ww0 ) +int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1, SurviveObject * tr0 , SurviveObject * ww0 ) { - struct SurviveContext * ctx = sv->ctx; + SurviveContext * ctx = sv->ctx; #ifdef HIDAPI if( !GlobalRXUSBMutx ) { @@ -488,10 +488,10 @@ int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, s return 0; } -int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen ) +int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen ) { int r; - struct SurviveViveData * sv = drv; + SurviveViveData * sv = drv; printf( "*CALLING %p %p\n", ctx, sv ); //XXX TODO: Handle haptics, etc. @@ -583,7 +583,7 @@ int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_c return 0; } -void survive_vive_usb_close( struct SurviveViveData * sv ) +void survive_vive_usb_close( SurviveViveData * sv ) { int i; #ifdef HIDAPI @@ -608,7 +608,7 @@ void survive_vive_usb_close( struct SurviveViveData * sv ) #endif } -int survive_vive_usb_poll( struct SurviveContext * ctx, void * v ) +int survive_vive_usb_poll( SurviveContext * ctx, void * v ) { #ifdef HIDAPI OGUnlockMutex( GlobalRXUSBMutx ); @@ -616,11 +616,11 @@ int survive_vive_usb_poll( struct SurviveContext * ctx, void * v ) OGUnlockMutex( GlobalRXUSBMutx ); return 0; #else - struct SurviveViveData * sv = v; + SurviveViveData * sv = v; int r = libusb_handle_events( sv->usbctx ); if( r ) { - struct SurviveContext * ctx = sv->ctx; + SurviveContext * ctx = sv->ctx; SV_ERROR( "Libusb poll failed." ); } return r; @@ -629,9 +629,9 @@ int survive_vive_usb_poll( struct SurviveContext * ctx, void * v ) } -int survive_get_config( char ** config, struct SurviveViveData * sv, int devno, int iface, int send_extra_magic ) +int survive_get_config( char ** config, SurviveViveData * sv, int devno, int iface, int send_extra_magic ) { - struct SurviveContext * ctx = sv->ctx; + SurviveContext * ctx = sv->ctx; int ret, count = 0, size = 0; uint8_t cfgbuff[64]; uint8_t compressed_data[8192]; @@ -749,7 +749,7 @@ int survive_get_config( char ** config, struct SurviveViveData * sv, int devno, #define POP2 (*(((uint16_t*)((readdata+=2)-2)))) #define POP4 (*(((uint32_t*)((readdata+=4)-4)))) -static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) +static void handle_watchman( SurviveObject * w, uint8_t * readdata ) { uint8_t startread[29]; @@ -1015,7 +1015,7 @@ void survive_data_cb( SurviveUSBInterface * si ) { case USB_IF_HMD: { - struct SurviveObject * headset = obj; + SurviveObject * headset = obj; readdata+=2; headset->buttonmask = POP1; //Lens headset->axis2 = POP2; //Lens Separation -- cgit v1.2.3 From af81a4dbe0e58e94b61b11c14b21a2decc15d823 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 18 Mar 2017 13:15:22 -0400 Subject: Fix unititialized array. --- src/survive_vive.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/survive_vive.c b/src/survive_vive.c index 91efc2c..c8e2b55 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -920,7 +920,7 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) { uint8_t *marked; marked = alloca(nrtime); - memset( marked, 0, sizeof( marked ) ); + memset( marked, 0, sizeof( nrtime ) ); int i, parpl = 0; timecount--; int timepl = 0; @@ -935,6 +935,7 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) while( marked[timepl] ) timepl++; if( timepl > timecount ) { fault = 3; goto end; } //Ran off max of list. uint32_t endtime = times[timepl++]; + int end = timepl + adv; if( end > timecount ) { fault = 4; goto end; } //end referencing off list if( marked[end] > 0 ) { fault = 5; goto end; } //Already marked trying to be used. @@ -978,7 +979,7 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) end: { SurviveContext * ctx = w->ctx; - SV_INFO( "Light decoding fault: %d\n", fault ); + SV_INFO( "Light decoding fault: %d", fault ); } } } -- cgit v1.2.3 From f976a532970afecce640ddfba88a6f1b04ab34b8 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 18 Mar 2017 13:21:13 -0400 Subject: Wow... Actually initilize to right size. --- src/survive_vive.c | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/src/survive_vive.c b/src/survive_vive.c index c8e2b55..f6465b2 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -771,7 +771,7 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) qty-=2; int propset = 0; int doimu = 0; - + int i; if( (type & 0xf0) == 0xf0 ) { @@ -916,11 +916,12 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) LightcapElement les[10]; int lese = 0; //les's end + //Second, go through all LEDs and extract the lightevent from them. { uint8_t *marked; marked = alloca(nrtime); - memset( marked, 0, sizeof( nrtime ) ); + memset( marked, 0, nrtime ); int i, parpl = 0; timecount--; int timepl = 0; @@ -933,6 +934,17 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) led >>= 3; while( marked[timepl] ) timepl++; + +#ifdef DEBUG_WATCHMAN + int i; + printf( "TP %d TC: %d : ", timepl, timecount ); + for( i = 0; i < nrtime; i++ ) + { + printf( "%d", marked[i] ); + } + printf( "\n" ); +#endif + if( timepl > timecount ) { fault = 3; goto end; } //Ran off max of list. uint32_t endtime = times[timepl++]; -- cgit v1.2.3 From 91a6bb98e0531bda74efc682223de14a1787fede Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 18 Mar 2017 15:20:30 -0400 Subject: Fix histograms --- src/survive_cal.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/survive_cal.c b/src/survive_cal.c index 0eb9446..9dc3af2 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -272,6 +272,7 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin if( sensors_visible < MIN_SENSORS_VISIBLE_PER_LH_FOR_CAL ) { //printf( "Dev %d, LH %d not enough visible points found.\n", i, j ); + reset_calibration( cd ); cd->found_common = 0; return; } @@ -440,7 +441,7 @@ static void handle_calibration( struct SurviveCalData *cd ) FLT stddevlen = 0; #define HISTOGRAMSIZE 31 - #define HISTOGRAMBINANG 0.00001 //TODO: Tune + #define HISTOGRAMBINANG ((3.14159)/400000.0) //TODO: Tune int histo[HISTOGRAMSIZE]; memset( histo, 0, sizeof( histo ) ); -- cgit v1.2.3 From 67ccb935ef12d9ab25f8223dea5208b96ae5052f Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 18 Mar 2017 15:20:50 -0400 Subject: increase time for integrating points. --- src/survive_cal.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/survive_cal.h b/src/survive_cal.h index 3d62051..701dc60 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -35,7 +35,9 @@ void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t t #define MAX_SENSORS_TO_CAL 96 #define MIN_PTS_BEFORE_CAL 24 -#define DRPTS 128 + + +#define DRPTS 1024 //Number of samples required in collection phase. #define POSE_OBJECTS 3 -- cgit v1.2.3 From 0f94e39d96fbc2744e61b85d2884250d0783eb11 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 18 Mar 2017 15:21:24 -0400 Subject: Add more possible debugging if turned on. --- src/survive_data.c | 22 ++++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/src/survive_data.c b/src/survive_data.c index 5c5a5e9..0789203 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -8,17 +8,31 @@ //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. void handle_lightcap( SurviveObject * so, LightcapElement * le ) { - SurviveContext * ctx = so->ctx; + SurviveContext * ctx = so->ctx; //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; - //if( so->codename[0] != 'H' ) - - if( le->sensor_id > SENSORS_PER_OBJECT ) { return; } +#if 0 + if( so->codename[0] == 'H' ) + { + static int lt; + static int last; + if( le->length > 1000 ) + { + int dl = le->timestamp - lt; + lt = le->timestamp; + if( dl > 10000 || dl < -10000 ) + printf( "+++%s %3d %5d %9d ", so->codename, le->sensor_id, le->length, dl ); + if( dl > 100000 ) printf(" \n" ); + } + last=le->length; + } +#endif + so->tsl = le->timestamp; if( le->length < 20 ) return; ///Assuming 20 is an okay value for here. -- cgit v1.2.3 From d4e7bc4037f9e0b4af3f11940b53520eb9f82ff6 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sat, 18 Mar 2017 20:25:06 -0400 Subject: fix compile --- src/survive_data.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/survive_data.c b/src/survive_data.c index 16a22e9..ffeacff 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -16,8 +16,6 @@ int32_t decode_acode(uint32_t length, int32_t main_divisor) { return (acode>>1) - 6; } -} - //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. void handle_lightcap( SurviveObject * so, LightcapElement * le ) { -- cgit v1.2.3 From aaaa45dcd79e1d9ebe04ec428422fa29d1961488 Mon Sep 17 00:00:00 2001 From: CNLohr Date: Sun, 19 Mar 2017 15:34:24 -0400 Subject: Closes #42 with logo. --- README.md | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index fcde197..7b89c2d 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,10 @@ # libsurvive -**WARNING PROJECT IN EXPERIMENTAL PHASE** +![Logo](https://cloud.githubusercontent.com/assets/2748168/24084003/9095c98a-0cb8-11e7-88a3-575f9f4c7bb4.png) -Discord: https://discordapp.com/invite/7QbCAGS +An Open-Source tool for working with lighthouse-based trakcing data, including support for the HTC Vive, which is still in the experimental phase. + +Most of the development is discussed on Discord. Join the chat and discussion here: https://discordapp.com/invite/7QbCAGS ## Livestream collection | Note | Youtube URL | Run time | -- cgit v1.2.3 From a229dec7ee46873e0b590fdbd34feb29112e4f0d Mon Sep 17 00:00:00 2001 From: CNLohr Date: Sun, 19 Mar 2017 16:55:54 -0400 Subject: Update README.md --- README.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/README.md b/README.md index 7b89c2d..db1d6ce 100644 --- a/README.md +++ b/README.md @@ -77,3 +77,9 @@ I may or may not read data from the Vive regarding configuration. If I do, it w The limiting factor for Vive viability on a given computer is the maximum available pixel clock frequency, and frequency limitations of the HDMI port, and HDMI and DisplayPort video cables. DisplayPort can support higher frequencies than HDMI, on Ivy Bridge HD4000 graphics. In fact, the Vive works with HD4000 graphics using DisplayPort, with native EDID resolution (2160x1200@90Hz). To support the Vive on HDMI, you either need a newer version of HDMI, or you need to define a custom resolution that respects pixel clock and video port limits, and is also accepted and displayed by the Vive. So far, we have not had success using custom resolutions on linux or on Windows. Windows imposes additional limitations in the form of restriction of WHQL certified drivers forbidden from using custom display resolutions (only allowing those defined by EDID in the monitor). Intel has released uncertified beta drivers for Haswell and newer processors, which should be able to support custom resolutions for the Vive (untested at this time). + + + +## Addendum and notes + +Thanks to Mr. Fault for our logo! -- cgit v1.2.3 From 8253e3c3adbb54ebc3d3c8e02c017e900a83edb0 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Mon, 20 Mar 2017 01:42:32 -0400 Subject: log synctimes. --- Makefile | 6 +++++ calibrate.c | 2 +- include/libsurvive/poser.h | 1 + src/survive_cal.c | 56 +++++++++++++++++++++++++++++++++++++++++++++- src/survive_cal.h | 3 +++ src/survive_data.c | 10 ++++++++- src/survive_process.c | 3 +++ 7 files changed, 78 insertions(+), 3 deletions(-) diff --git a/Makefile b/Makefile index a642877..1899c4c 100644 --- a/Makefile +++ b/Makefile @@ -32,6 +32,12 @@ LIBSURVIVE_CORE:=$(LIBSURVIVE_CORE) LIBSURVIVE_O:=$(POSERS) $(REDISTS) $(LIBSURVIVE_CORE) LIBSURVIVE_C:=$(LIBSURVIVE_O:.o=.c) + +#If you want to use HIDAPI on Linux. +#CFLAGS:=$(CFLAGS) -DHIDAPI +#REDISTS:=$(REDISTS) redist/hid-linux.o +#LDFLAGS:=$(LDFLAGS) -ludev + #Useful Preprocessor Directives: # -DUSE_DOUBLE = use double instead of float for most operations. # -DNOZLIB = use puff.c diff --git a/calibrate.c b/calibrate.c index 726a8cc..f27131a 100644 --- a/calibrate.c +++ b/calibrate.c @@ -56,7 +56,7 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int // if( timeinsweep < 0 ) return; survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length ); if( sensor_id < 0 ) return; - if( acode == -1 ) return; + if( acode < 0 ) return; //return; int jumpoffset = sensor_id; if( strcmp( so->codename, "WM0" ) == 0 ) jumpoffset += 32; diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h index 98c926e..cf11e0c 100644 --- a/include/libsurvive/poser.h +++ b/include/libsurvive/poser.h @@ -45,6 +45,7 @@ typedef struct //If "lengths[...]" < 0, means not a valid piece of sweep information. FLT lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; FLT angles [SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; //2 Axes (Angles in LH space) + FLT synctimes[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES]; PoserDataIMU lastimu; } PoserDataFullScene; diff --git a/src/survive_cal.c b/src/survive_cal.c index 9dc3af2..04931cc 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -196,6 +196,21 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //If all lighthouses have their OOTX set, move on. } break; + case 3: //Look for light sync lengths. + { + if( acode >= -2 ) break; + else if( acode < -4 ) break; + int lh = (-acode) - 3; + + if( strcmp( so->codename, "WM0" ) == 0 ) + sensor_id += 32; + if( strcmp( so->codename, "WM1" ) == 0 ) + sensor_id += 64; + + cd->all_sync_times[sensor_id][lh][cd->all_sync_counts[sensor_id][lh]++] = length; + break; + } + } } @@ -333,6 +348,8 @@ static void reset_calibration( struct SurviveCalData * cd ) cd->found_common = 0; cd->times_found_common = 0; cd->stage = 2; + + memset( cd->all_sync_counts, 0, sizeof( cd->all_sync_counts ) ); } static void handle_calibration( struct SurviveCalData *cd ) @@ -358,9 +375,45 @@ static void handle_calibration( struct SurviveCalData *cd ) #else mkdir( "calinfo", 0755 ); #endif + int sen, axis, lh; + FLT temp_syncs[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES]; + + //Just to get it out of the way early, we'll calculate the sync-pulse-lengths here. + FILE * sync_time_info = fopen( "calinfo/synctime.csv", "w" ); + + for( sen = 0; sen < MAX_SENSORS_TO_CAL; sen++ ) + for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ ) + { + int count = cd->all_sync_counts[sen][lh]; + int i; + double totaltime; + + totaltime = 0; + + if( count < 20 ) continue; + for( i = 0; i < count; i++ ) + { + totaltime += cd->all_sync_times[sen][lh][i]; + } + FLT avg = totaltime/count; + + double stddev = 0.0; + for( i = 0; i < count; i++ ) + { + stddev += (cd->all_sync_times[sen][lh][i] - avg)*(cd->all_sync_times[sen][lh][i] - avg); + } + stddev /= count; + + fprintf( sync_time_info, "%d %d %f %d %f\n", sen, lh, totaltime/count, count, stddev ); + } + + fclose( sync_time_info ); + + + + FILE * hists = fopen( "calinfo/histograms.csv", "w" ); FILE * ptinfo = fopen( "calinfo/ptinfo.csv", "w" ); - int sen, axis, lh; for( sen = 0; sen < MAX_SENSORS_TO_CAL; sen++ ) for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ ) for( axis = 0; axis < 2; axis++ ) @@ -526,6 +579,7 @@ static void handle_calibration( struct SurviveCalData *cd ) fsd.lengths[i][j][1] = cd->avglens[dataindex+1]; fsd.angles[i][j][0] = cd->avgsweeps[dataindex+0]; fsd.angles[i][j][1] = cd->avgsweeps[dataindex+1]; + fsd.synctimes[i][j] = temp_syncs[i][j]; } int r = cd->ConfigPoserFn( cd->poseobjects[obj], (PoserData*)&fsd ); diff --git a/src/survive_cal.h b/src/survive_cal.h index 701dc60..c64b8f9 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -56,6 +56,9 @@ struct SurviveCalData int8_t found_common; int8_t times_found_common; + FLT all_sync_times[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][DRPTS]; + int16_t all_sync_counts[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES]; + //For camfind (4+) //Index is calculated with: int dataindex = sen*(2*NUM_LIGHTHOUSES)+lh*2+axis; FLT avgsweeps[MAX_CAL_PT_DAT]; diff --git a/src/survive_data.c b/src/survive_data.c index ffeacff..8c5c646 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -69,7 +69,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) { int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length; - //printf("m sync %d %d %d %d\n", le->sensor_id, so->last_sync_time[ssn], le->timestamp, delta); + so->did_handle_ootx = 0; @@ -114,6 +114,14 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) } } } + + //Extra tidbit for storing length-of-sync-pulses. + { + int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz. + int base_station = is_new_pulse; + printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); + ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length ); + } } diff --git a/src/survive_process.c b/src/survive_process.c index 8dc849a..9295638 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -16,6 +16,9 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length ); } + //We don't use sync times, yet. + if( acode < -1 ) return; + if( base_station > NUM_LIGHTHOUSES ) return; //No loner need sync information past this point. -- cgit v1.2.3 From 3694605b23b1019488844bf448d461eacc69077f Mon Sep 17 00:00:00 2001 From: cnlohr Date: Wed, 22 Mar 2017 00:12:31 -0400 Subject: Update RawDraw to version that can at least support OpenGL if we want it. --- redist/CNFGFunctions.c | 82 ++++++++++++++++++++++++++++++++++++++++++++++++++ redist/CNFGFunctions.h | 7 ++++- redist/CNFGWinDriver.c | 71 ++++++++++++++++++++++++++++++++++++++++--- redist/CNFGXDriver.c | 74 ++++++++++++++++++++++++++++++++++++++++----- 4 files changed, 221 insertions(+), 13 deletions(-) diff --git a/redist/CNFGFunctions.c b/redist/CNFGFunctions.c index 947456f..e93eb6f 100644 --- a/redist/CNFGFunctions.c +++ b/redist/CNFGFunctions.c @@ -270,3 +270,85 @@ void CNFGDrawTextbox( int x, int y, const char * text, int textsize ) CNFGPenY = y + textsize; CNFGDrawText( text, textsize ); } + + +#ifdef CNFGOGL + +#include + +uint32_t CNFGColor( uint32_t RGB ) +{ + unsigned char red = RGB & 0xFF; + unsigned char grn = ( RGB >> 8 ) & 0xFF; + unsigned char blu = ( RGB >> 16 ) & 0xFF; + glColor3ub( red, grn, blu ); +} + +void CNFGClearFrame() +{ + short w, h; + unsigned char red = CNFGBGColor & 0xFF; + unsigned char grn = ( CNFGBGColor >> 8 ) & 0xFF; + unsigned char blu = ( CNFGBGColor >> 16 ) & 0xFF; + glClearColor( red/255.0, grn/255.0, blu/255.0, 1.0 ); + glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); + CNFGGetDimensions( &w, &h ); + glMatrixMode( GL_PROJECTION ); + glLoadIdentity(); + glViewport( 0, 0, w, h ); + glOrtho( 0, w, h, 0, 1, -1 ); + glMatrixMode( GL_MODELVIEW ); + glLoadIdentity(); +} + + +void CNFGTackSegment( short x1, short y1, short x2, short y2 ) +{ + if( x1 == x2 && y1 == y2 ) + { + glBegin( GL_POINTS ); + glVertex2f( x1+.5, y1+.5 ); + glEnd(); + } + else + { + glBegin( GL_LINES ); + glVertex2f( x1+.5, y1+.5 ); + glVertex2f( x2+.5, y2+.5 ); + glEnd(); + } +} + +void CNFGTackPixel( short x1, short y1 ) +{ + glBegin( GL_POINTS ); + glVertex2f( x1, y1 ); + glEnd(); +} + +void CNFGTackRectangle( short x1, short y1, short x2, short y2 ) +{ + glBegin( GL_QUADS ); + glVertex2f( x1, y1 ); + glVertex2f( x2, y1 ); + glVertex2f( x2, y2 ); + glVertex2f( x1, y2 ); + glEnd(); +} + +void CNFGTackPoly( RDPoint * points, int verts ) +{ + int i; + glBegin( GL_TRIANGLE_FAN ); + glVertex2f( points[0].x, points[0].y ); + for( i = 1; i < verts; i++ ) + { + glVertex2f( points[i].x, points[i].y ); + } + glEnd(); +} + +void CNFGInternalResize( short x, short y ) { } + + +#endif diff --git a/redist/CNFGFunctions.h b/redist/CNFGFunctions.h index 9ecb1bd..179a20b 100644 --- a/redist/CNFGFunctions.h +++ b/redist/CNFGFunctions.h @@ -1,4 +1,4 @@ -//Copyright (c) 2011 <>< Charles Lohr - Under the MIT/x11 or NewBSD License you choose. +//Copyright (c) 2011, 2017 <>< Charles Lohr - Under the MIT/x11 or NewBSD License you choose. #ifndef _DRAWFUCNTIONS_H #define _DRAWFUCNTIONS_H @@ -49,6 +49,11 @@ void HandleDestroy(); //Internal function for resizing rasterizer for rasterizer-mode. void CNFGInternalResize( short x, short y ); //don't call this. +//Not available on all systems. Use The OGL portion with care. +#ifdef CNFGOGL +void CNFGSetVSync( int vson ); +void * CNFGGetExtension( const char * extname ); +#endif #ifdef __cplusplus }; diff --git a/redist/CNFGWinDriver.c b/redist/CNFGWinDriver.c index a029419..c5da925 100644 --- a/redist/CNFGWinDriver.c +++ b/redist/CNFGWinDriver.c @@ -13,7 +13,6 @@ static HWND lsHWND; static HDC lsWindowHDC; static HDC lsHDC; - #ifdef RASTERIZER #include "CNFGRasterizer.h" @@ -32,14 +31,34 @@ static void InternalHandleResize(); #endif +#ifdef CNFGOGL +#include +static HGLRC hRC=NULL; +static void InternalHandleResize() { } +void CNFGSwapBuffers() +{ + SwapBuffers(lsWindowHDC); +} +#endif + void CNFGGetDimensions( short * x, short * y ) { + static int lastx, lasty; + RECT window; + GetClientRect( lsHWND, &window ); + bufferx = ( window.right - window.left); + buffery = ( window.bottom - window.top); + if( bufferx != lastx || buffery != lasty ) + { + lastx = bufferx; + lasty = buffery; + InternalHandleResize(); + } *x = bufferx; *y = buffery; } - void CNFGUpdateScreenWithBitmap( unsigned long * data, int w, int h ) { RECT r; @@ -126,9 +145,49 @@ void CNFGSetup( const char * name_of_window, int width, int height ) hInstance, NULL); //no parameters to pass - lsWindowHDC = GetDC( lsHWND ); +#ifdef CNFGOGL + //From NeHe + static PIXELFORMATDESCRIPTOR pfd = + { + sizeof(PIXELFORMATDESCRIPTOR), + 1, + PFD_DRAW_TO_WINDOW | + PFD_SUPPORT_OPENGL | + PFD_DOUBLEBUFFER, + PFD_TYPE_RGBA, + 32, + 0, 0, 0, 0, 0, 0, + 0, + 0, + 0, + 0, 0, 0, 0, + 16, + 0, + 0, + PFD_MAIN_PLANE, + 0, + 0, 0, 0 + }; + GLuint PixelFormat = ChoosePixelFormat( lsWindowHDC, &pfd ); + if( !SetPixelFormat( lsWindowHDC, PixelFormat, &pfd ) ) + { + MessageBox( 0, "Could not create PFD for OpenGL Context\n", 0, 0 ); + exit( -1 ); + } + if (!(hRC=wglCreateContext(lsWindowHDC))) // Are We Able To Get A Rendering Context? + { + MessageBox( 0, "Could not create OpenGL Context\n", 0, 0 ); + exit( -1 ); + } + if(!wglMakeCurrent(lsWindowHDC,hRC)) // Try To Activate The Rendering Context + { + MessageBox( 0, "Could not current OpenGL Context\n", 0, 0 ); + exit( -1 ); + } +#endif + lsHDC = CreateCompatibleDC( lsWindowHDC ); lsBitmap = CreateCompatibleBitmap( lsWindowHDC, bufferx, buffery ); SelectObject( lsHDC, lsBitmap ); @@ -182,6 +241,7 @@ void CNFGHandleInput() } } +#ifndef CNFGOGL #ifndef RASTERIZER @@ -237,8 +297,7 @@ void CNFGClearFrame() DeleteObject( lsClearBrush ); lsClearBrush = CreateSolidBrush( CNFGBGColor ); SelectObject( lsHDC, lsClearBrush ); - - FillRect( lsHDC, &r, lsClearBrush ); + FillRect( lsHDC, &r, lsClearBrush); } void CNFGTackPoly( RDPoint * points, int verts ) @@ -287,3 +346,5 @@ void CNFGSwapBuffers() void CNFGInternalResize( short bufferx, short buffery ) { } #endif +#endif + diff --git a/redist/CNFGXDriver.c b/redist/CNFGXDriver.c index 8a8904a..ebaed91 100644 --- a/redist/CNFGXDriver.c +++ b/redist/CNFGXDriver.c @@ -1,4 +1,4 @@ -//Copyright (c) 2011 <>< Charles Lohr - Under the MIT/x11 or NewBSD License you choose. +//Copyright (c) 2011, 2017 <>< Charles Lohr - Under the MIT/x11 or NewBSD License you choose. //portions from //http://www.xmission.com/~georgeps/documentation/tutorials/Xlib_Beginner.html @@ -25,6 +25,17 @@ Window CNFGWindow; Pixmap CNFGPixmap; GC CNFGGC; GC CNFGWindowGC; +Visual * CNFGVisual; + + +#ifdef CNFGOGL +#include +#include + +GLXContext CNFGCtx; +void * CNFGGetExtension( const char * extname ) { return glXGetProcAddressARB((const GLubyte *) extname); } +#endif + int FullScreen = 0; void CNFGGetDimensions( short * x, short * y ) @@ -86,7 +97,7 @@ void CNFGSetupFullscreen( const char * WindowName, int screen_no ) exit( 1 ); } - Visual * visual = DefaultVisual(CNFGDisplay, screen); + CNFGVisual = DefaultVisual(CNFGDisplay, screen); CNFGWinAtt.depth = DefaultDepth(CNFGDisplay, screen); if (XineramaQueryExtension(CNFGDisplay, &a, &b ) && @@ -117,7 +128,9 @@ void CNFGSetupFullscreen( const char * WindowName, int screen_no ) CNFGWindow = XCreateWindow(CNFGDisplay, XRootWindow(CNFGDisplay, screen), xpos, ypos, CNFGWinAtt.width, CNFGWinAtt.height, - 0, CNFGWinAtt.depth, InputOutput, visual, CWBorderPixel | CWEventMask | CWOverrideRedirect | CWSaveUnder, &setwinattr); + 0, CNFGWinAtt.depth, InputOutput, CNFGVisual, + CWBorderPixel | CWEventMask | CWOverrideRedirect | CWSaveUnder, + &setwinattr); XMapWindow(CNFGDisplay, CNFGWindow); XSetInputFocus( CNFGDisplay, CNFGWindow, RevertToParent, CurrentTime ); @@ -155,7 +168,31 @@ void CNFGSetup( const char * WindowName, int w, int h ) XGetWindowAttributes( CNFGDisplay, RootWindow(CNFGDisplay, 0), &CNFGWinAtt ); int depth = CNFGWinAtt.depth; - CNFGWindow = XCreateWindow(CNFGDisplay, RootWindow(CNFGDisplay, 0), 1, 1, w, h, 0, depth, InputOutput, CopyFromParent, 0, 0 ); + int screen = DefaultScreen(CNFGDisplay); + CNFGVisual = DefaultVisual(CNFGDisplay, screen); + +#ifdef CNFGOGL + int attribs[] = { GLX_RGBA, + GLX_DOUBLEBUFFER, + GLX_RED_SIZE, 1, + GLX_GREEN_SIZE, 1, + GLX_BLUE_SIZE, 1, + GLX_DEPTH_SIZE, 1, + None }; + XVisualInfo * vis = glXChooseVisual(CNFGDisplay, screen, attribs); + CNFGVisual = vis->visual; + depth = vis->depth; + CNFGCtx = glXCreateContext( CNFGDisplay, vis, NULL, True ); +#endif + + XSetWindowAttributes attr; + attr.background_pixel = 0; + attr.border_pixel = 0; + attr.colormap = XCreateColormap( CNFGDisplay, RootWindow(CNFGDisplay, 0), CNFGVisual, AllocNone); + attr.event_mask = StructureNotifyMask | ExposureMask | KeyPressMask; + int mask = CWBackPixel | CWBorderPixel | CWColormap | CWEventMask; + + CNFGWindow = XCreateWindow(CNFGDisplay, RootWindow(CNFGDisplay, 0), 1, 1, w, h, 0, depth, InputOutput, CNFGVisual, mask, &attr ); XMapWindow(CNFGDisplay, CNFGWindow); XFlush(CNFGDisplay); @@ -163,7 +200,12 @@ void CNFGSetup( const char * WindowName, int w, int h ) Atom WM_DELETE_WINDOW = XInternAtom( CNFGDisplay, "WM_DELETE_WINDOW", False ); XSetWMProtocols( CNFGDisplay, CNFGWindow, &WM_DELETE_WINDOW, 1 ); + XSelectInput( CNFGDisplay, CNFGWindow, KeyPressMask | KeyReleaseMask | ButtonPressMask | ButtonReleaseMask | ExposureMask | PointerMotionMask ); + +#ifdef CNFGOGL + glXMakeCurrent( CNFGDisplay, CNFGWindow, CNFGCtx ); +#endif } void CNFGHandleInput() @@ -226,7 +268,6 @@ void CNFGUpdateScreenWithBitmap( unsigned long * data, int w, int h ) if( !xi ) { int screen = DefaultScreen(CNFGDisplay); - Visual * visual = DefaultVisual(CNFGDisplay, screen); depth = DefaultDepth(CNFGDisplay, screen)/8; // xi = XCreateImage(CNFGDisplay, DefaultVisual( CNFGDisplay, DefaultScreen(CNFGDisplay) ), depth*8, ZPixmap, 0, (char*)data, w, h, 32, w*4 ); // lw = w; @@ -236,7 +277,7 @@ void CNFGUpdateScreenWithBitmap( unsigned long * data, int w, int h ) if( lw != w || lh != h ) { if( xi ) free( xi ); - xi = XCreateImage(CNFGDisplay, DefaultVisual( CNFGDisplay, DefaultScreen(CNFGDisplay) ), depth*8, ZPixmap, 0, (char*)data, w, h, 32, w*4 ); + xi = XCreateImage(CNFGDisplay, CNFGVisual, depth*8, ZPixmap, 0, (char*)data, w, h, 32, w*4 ); lw = w; lh = h; } @@ -247,7 +288,25 @@ void CNFGUpdateScreenWithBitmap( unsigned long * data, int w, int h ) } -#ifndef RASTERIZER +#ifdef CNFGOGL + +void CNFGSetVSync( int vson ) +{ + void (*glfn)( int ); + glfn = (void (*)( int ))CNFGGetExtension( "glXSwapIntervalMESA" ); if( glfn ) glfn( vson ); + glfn = (void (*)( int ))CNFGGetExtension( "glXSwapIntervalSGI" ); if( glfn ) glfn( vson ); + glfn = (void (*)( int ))CNFGGetExtension( "glXSwapIntervalEXT" ); if( glfn ) glfn( vson ); +} + +void CNFGSwapBuffers() +{ + glFlush(); + glFinish(); + glXSwapBuffers( CNFGDisplay, CNFGWindow ); +} +#endif + +#if !defined( RASTERIZER ) && !defined( CNFGOGL) uint32_t CNFGColor( uint32_t RGB ) @@ -302,3 +361,4 @@ void CNFGInternalResize( short x, short y ) { } #include "CNFGRasterizer.h" #endif + -- cgit v1.2.3 From c72b1dd68a653dff2be9915750d2f6535a6db3bd Mon Sep 17 00:00:00 2001 From: CNLohr Date: Wed, 22 Mar 2017 04:44:15 -0400 Subject: Fix Windows compile if OGL is turned on. --- redist/CNFGFunctions.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/redist/CNFGFunctions.c b/redist/CNFGFunctions.c index e93eb6f..25e9298 100644 --- a/redist/CNFGFunctions.c +++ b/redist/CNFGFunctions.c @@ -274,6 +274,10 @@ void CNFGDrawTextbox( int x, int y, const char * text, int textsize ) #ifdef CNFGOGL +#ifdef _MSC_VER +#include +#pragma comment( lib, "OpenGL32.lib" ) +#endif #include uint32_t CNFGColor( uint32_t RGB ) -- cgit v1.2.3 From 39ef5af74702c8825a82f65cf68e6af875a814ee Mon Sep 17 00:00:00 2001 From: ultramn Date: Thu, 23 Mar 2017 00:41:22 -0400 Subject: Added a dynamic plotting tool for OrthoSolve. Charles added orthogonalization to the rotation matrix. --- dave/AffineSolve | Bin 65424 -> 40104 bytes dave/AffineSolve.c | 1241 +++++++++++++++++++++++++------------------------ dave/Makefile | 20 +- dave/OrthoPlot.c | 451 ++++++++++++++++++ dave/dclapack_test.c | 2 +- dave/fileutil.c | 133 ++++++ dave/fileutil.h | 35 ++ src/poser_daveortho.c | 85 +++- src/survive_cal.h | 2 +- src/survive_data.c | 2 +- 10 files changed, 1356 insertions(+), 615 deletions(-) create mode 100644 dave/OrthoPlot.c create mode 100644 dave/fileutil.c create mode 100644 dave/fileutil.h diff --git a/dave/AffineSolve b/dave/AffineSolve index 98a9590..7d51b34 100755 Binary files a/dave/AffineSolve and b/dave/AffineSolve differ diff --git a/dave/AffineSolve.c b/dave/AffineSolve.c index 4fba56b..685062e 100644 --- a/dave/AffineSolve.c +++ b/dave/AffineSolve.c @@ -11,10 +11,13 @@ #include #include "dclapack.h" #include +#define RegisterDriver(a,b) +#include "poser_daveortho.c" #define LH_ID 0 #define NUM_HMD 32 +#define INDIR "full_test_triangle_on_floor/" -#define MAX_POINTS 128 +#define MAX_POINTS SENSORS_PER_OBJECT //#define _ABS(a) ( (a)<=0 ? -(a) : (a) ) #define _SIGN(a) ( (a)<=0 ? -1.0f : 1.0f ) #define RANDF ( (float)rand() / (float)RAND_MAX ) @@ -31,7 +34,7 @@ float hmd_pos[NUM_HMD][3]; void ReadHmdPoints() { int i; - FILE *fin = fopen("HMD_points.csv","r"); + FILE *fin = fopen(INDIR "HMD_points.csv","r"); if (fin==NULL) { printf("ERROR: could not open HMD_points.csv for reading\n"); exit(1); @@ -52,7 +55,7 @@ void ReadPtinfo() for (i=0; i TOO_SMALL) { + float scaleRot = stepSizeRot / gradSizeRot; + for (j=0; j<3; j++) { + for (k=0; k<3; k++) { + T[j][k] += scaleRot * conj[j][k]; + } + } + stepSizeRot *= falloff; + } + + if (gradSizePos > TOO_SMALL) { + float scalePos = stepSizePos / gradSizePos; + for (j=0; j<3; j++) { + T[j][3] += scalePos * conj[j][3]; + } + stepSizePos *= falloff; + } + + // Constrain the gradient (such that scaling is one) + if (constrain) + { + // Measure the scales + float len[3] = {0.0, 0.0, 0.0}; + for (j=0; j<3; j++) { + double lenSq = 0.0; + for (k=0; k<3; k++) { lenSq += (double)T[j][k] * (double)T[j][k]; } + len[j] = sqrt(lenSq); + } - // 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*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; + // How far off is the scale? + float xzLen = 0.5 * (len[0] + len[2]); + if (xzLen > TOO_SMALL) { + float inv_xzLen = 1.0 / xzLen; + for (j=0; j<3; j++) { + T[3][j] *= inv_xzLen; + } + } + + // Rescale the thing + for (j=0; j<3; j++) + { + if (len[j] > TOO_SMALL) { + float inv_len = 1.0 / len[j]; + for (k=0; k<3; k++) { T[j][k] *= inv_len; } + } } - y_y = -y_y; } - x_y = -x_y; } - printf("bestErr %f\n", bestErr); -*/ + float dist = sqrt(T[0][3]*T[0][3] + T[1][3]*T[1][3] + T[2][3]*T[2][3]); + printf("AffineSolve: pos: %f %f %f dist: %f\n", T[0][3], T[1][3], T[2][3], dist); +} + +int main() +{ + int i,j,k,sen,axis; + + // Read the data files + ReadHmdPoints(); + ReadPtinfo(); //------------------------- - // A test version of the rescaling to the proper length + // Package the lighthouse data for "AffineSolve" //------------------------- - FLOAT ydist2; - FLOAT bestBestErr = 9999.0; - FLOAT bestYdist = 0; - for (ydist2=ydist-0.1; ydist2 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); + for (sen=0; sen 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; } -/* } else { - // Calculate "beta" for Fletcher Reeves method - float beta = gradDot / prevGradDot; -//printf("gradDot %f prevGradDot %f beta %f\n", gradDot, prevGradDot, beta); - - // Update the conjugate - for (j=0; j<3; j++) { - for (k=0; k<4; k++) { - conj[j][k] = beta*conj[j][k] - de_dT[j][k]; - } - } + y_y = -y_y; } + x_y = -x_y; + } + printf("bestErr %f\n", bestErr); */ -// PRINT_MAT(de_dT,4,4); -// exit(1); + //------------------------- + // A test version of the rescaling to the proper length + //------------------------- + FLOAT ydist2; + FLOAT bestBestErr = 9999.0; + FLOAT bestYdist = 0; + for (ydist2=ydist-0.1; ydist2 TOO_SMALL) { - float scaleRot = stepSizeRot / gradSizeRot; - for (j=0; j<3; j++) { - for (k=0; k<3; k++) { - T[j][k] += scaleRot * conj[j][k]; - } - } - stepSizeRot *= falloff; - } - - if (gradSizePos > TOO_SMALL) { - float scalePos = stepSizePos / gradSizePos; - for (j=0; j<3; j++) { - T[j][3] += scalePos * conj[j][3]; - } - stepSizePos *= falloff; - } - - // Constrain the gradient (such that scaling is one) - if (constrain) - { - // Measure the scales - float len[3] = {0.0, 0.0, 0.0}; - for (j=0; j<3; j++) { - double lenSq = 0.0; - for (k=0; k<3; k++) { lenSq += (double)T[j][k] * (double)T[j][k]; } - len[j] = sqrt(lenSq); - } - - // How far off is the scale? - float xzLen = 0.5 * (len[0] + len[2]); - if (xzLen > TOO_SMALL) { - float inv_xzLen = 1.0 / xzLen; - for (j=0; j<3; j++) { - T[3][j] *= inv_xzLen; - } - } + // 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++) { - // Rescale the thing - for (j=0; j<3; j++) - { - if (len[j] > TOO_SMALL) { - float inv_len = 1.0 / len[j]; - for (k=0; k<3; k++) { T[j][k] *= inv_len; } + // 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; + bestYdist = ydist2; } } - 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); -} + ydist = bestYdist; -int main() -{ - int i,j,k,sen,axis; - - // Read the data files - ReadHmdPoints(); - ReadPtinfo(); +/* + for (i=0; i // Standard Header For Most Programs +#include +#include +#include +#include "os_generic.h" +#include "linmath.h" +#include "fileutil.h" + +#ifdef __APPLE__ +#include // The GL Header File +#include // The GL Utility Toolkit (Glut) Header +#else +#include +#include +#endif +#ifdef __linux__ +#include +#endif + +#define RegisterDriver(a,b) +#include "poser_daveortho.c" + + +// Required to set up a window +#define WIDTH 1280 +#define HEIGHT 1280 +#define FULLSCREEN 0 +int keys[256]; // Regular keyboard keys +int sp_keys[256]; // Special keyboard keycodes (GLUT specific) + +#define LH_ID 0 +#define NUM_HMD 32 +#define INDIR "dave/full_test_triangle_on_floor/" +#define MAX_POINTS SENSORS_PER_OBJECT + +#define PI 3.1415926535897932386264 +#define MOVESPEED 1.0 +#define ROTSPEED 5.0 + +// View space +float posx=0.0f; +float posy=0.0f; +float posz=0.0f; +float rotx=0.0f; +float roty=0.0f; +float rotz=0.0f; + +// Data for the "fake" ortho solve formula +float Tortho[4][4]; // OUTPUT: 4x4 transformation matrix +FLOAT S_out[2][MAX_POINTS]; // INPUT: array of screenspace points +FLOAT S_in[2][MAX_POINTS]; // INPUT: array of screenspace points +FLOAT X_in[3][MAX_POINTS]; // INPUT: array of offsets +int nPoints=0; + +//-------------------------------------------------------------------- +// +//-------------------------------------------------------------------- + +void DrawGrid( + float minX, float maxX, + float minY, float maxY, + float minZ, float maxZ, + float stepX, float stepY, float stepZ); + +void DrawCoordinateSystem( + float x, float y, float z, + float qx, float qy, float qz, float qr); + + +float hmd_pos[NUM_HMD][3]; +void ReadHmdPoints() +{ + int i; + FILE *fin = fopen(INDIR "HMD_points.csv","r"); + if (fin==NULL) { + printf("ERROR: could not open " INDIR "HMD_points.csv for reading\n"); + exit(1); + } + + for (i=0; i= read_frameno-6 && + read_hmdAngleViewed[sweepy][i] >= read_frameno-6 && + hmdAngles[sweepx][i]!=-9999.0 && hmdAngles[sweepy][i]!=-9999.0) + { + S_in[0][nPoints] = hmdAngles[sweepy][i]; + S_in[1][nPoints] = hmdAngles[sweepx][i]; + X_in[0][nPoints] = hmd_pos[i][0]; + X_in[1][nPoints] = hmd_pos[i][1]; + X_in[2][nPoints] = hmd_pos[i][2]; +printf("i %d S %f %f X %f %f %f frno %d %d currfr %d\n", + i, S_in[0][nPoints], S_in[1][nPoints], + X_in[0][nPoints], X_in[1][nPoints], X_in[2][nPoints], + read_hmdAngleViewed[sweepx][i], read_hmdAngleViewed[sweepy][i], read_frameno); + nPoints++; + } + } + + read_frameno++; + + //-------------------------------------------------- + // Run the "OrthoSolve" and then the "AffineSolve" + //-------------------------------------------------- + + // Run OrthoSolve + OrthoSolve( + Tortho, // OUTPUT: 4x4 transformation matrix + S_out, // OUTPUT: array of output screenspace points + S_in, // INPUT: array of screenspace points + X_in, // INPUT: array of offsets + nPoints); + printf( "POS: %f %f %f\n", Tortho[0][3], Tortho[1][3], Tortho[2][3]); + + + //------------------------ + // Draw the inputs + //------------------------ + glPointSize(3.0); + + // Draw the input points + glColor3f(1.0,0.5,0.5); + glBegin(GL_POINTS); + for (i=0; i #include diff --git a/dave/fileutil.c b/dave/fileutil.c new file mode 100644 index 0000000..04dc241 --- /dev/null +++ b/dave/fileutil.c @@ -0,0 +1,133 @@ +#include "fileutil.h" +#include +#include + +#define PI 3.14159265358979323846264 + + +og_mutex_t read_mutex; +og_thread_t read_thread; +double read_hmdAngles[NUM_SWEEP][NUM_HMD]; +int read_hmdAngleViewed[NUM_SWEEP][NUM_HMD]; +int read_frameno=0; + +static FILE *fopen_orDie(const char *path, const char *flag) +{ + FILE *f = fopen(path, flag); + if (f == NULL) { + printf("ERROR could not oepn %s for %s\n", path, flag); + exit(1); + } + return f; +} + +static void SeekToken(FILE *f, const char *keyword) +{ + char token[4096]; + do { + fscanf(f, "%s", token); + } while( strcmp(token,keyword)!=0 && !feof(f) ); +} + +void LoadLighthousePos( + const char *path, + float *x, float *y, float *z, + float *qi, float *qj, float *qk, float *qreal) +{ + FILE *f = fopen_orDie(path,"r"); + SeekToken(f, "POS:"); + fscanf(f, "%f %f %f\n", x, y, z); + SeekToken(f, "QUAT:"); + fscanf(f, "%f %f %f %f\n", qreal, qi, qj, qk); + fclose (f); +} + +void LoadHmdProcessedDataAngles( + const char *path, + double angles[NUM_SWEEP][NUM_HMD]) +{ + int i,j; + char type[256]; + char sweep[256]; + int id; + int nSweep; + double ang; + double d1,d2,d3; // revisit these numbers later + + // Initialize all of the angles to -9999 + for (i=0; iNUM_HMD) { continue; } + + OGLockMutex (read_mutex); + read_hmdAngles[sweepId][id] = angle; + OGUnlockMutex(read_mutex); + read_hmdAngleViewed[sweepId][id] = read_frameno; + } + } +} + + diff --git a/dave/fileutil.h b/dave/fileutil.h new file mode 100644 index 0000000..e5da244 --- /dev/null +++ b/dave/fileutil.h @@ -0,0 +1,35 @@ +#ifndef _fileutil_h_ +#define _fileutil_h_ + +#include +#include "os_generic.h" + +void LoadLighthousePos( + const char *path, + float *x, float *y, float *z, + float *qi, float *qj, float *qk, float *qreal); + + +// first 32 are hmd, next 24 wm0 next 24 wm1 +#define NUM_HMD 80 +#define NUM_SWEEP 4 +#define SWEEP_LX 0 +#define SWEEP_LY 1 +#define SWEEP_RX 2 +#define SWEEP_RY 3 +void LoadHmdProcessedDataAngles( + const char *path, + double angle[NUM_SWEEP][NUM_HMD]); + + +extern og_mutex_t read_mutex; +extern og_thread_t read_thread; +extern double read_hmdAngles[NUM_SWEEP][NUM_HMD]; +extern int read_hmdAngleViewed[NUM_SWEEP][NUM_HMD]; +extern int read_frameno; +void *ThreadReadHmtAngles(void *junk); + + +#endif // __fileutil_h_ + + diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index e81e154..80f65a9 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -265,7 +265,8 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); // FLT ydist1 = 1.0 / uhat_len; //0.25*PI / uhat_len; // FLT ydist2 = 1.0 / rhat_len; //0.25*PI / rhat_len; FLT ydist = 1.0 / urhat_len; - //printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist); + printf("ydist %f\n", ydist); +// printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist); //-------------------- // Rescale the axies to be of the proper length @@ -282,7 +283,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); if( x_y != x_y ) x_y = 0; if( y_y != y_y ) y_y = 0; if( z_y != z_y ) z_y = 0; -/* + // Exhaustively flip the minus sign of the z axis until we find the right one . . . FLT bestErr = 9999.0; FLT xy_dot2 = x[0][0]*y[0][0] + x[2][0]*y[2][0]; @@ -302,7 +303,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); FLT cx,cy,cz; CrossProduct(cx,cy,cz,x[0][0],x_y,x[2][0],y[0][0],y_y,y[2][0]); FLT hand = cx*z[0][0] + cy*z_y + cz*z[2][0]; - printf("err %f hand %f\n", err, hand); +// printf("err %f hand %f\n", err, hand); // If we are the best right-handed frame so far //if (hand > 0 && err < bestErr) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; } @@ -313,9 +314,9 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); } x_y = -x_y; } - printf("bestErr %f\n", bestErr); -*/ +// printf("bestErr %f\n", bestErr); +/* //------------------------- // A test version of the rescaling to the proper length //------------------------- @@ -338,7 +339,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); if( y_y != y_y ) y_y = 0; if( z_y != z_y ) z_y = 0; - printf( "---> %f %f %f\n", x_y, y_y, z_y ); +// printf( "---> %f %f %f\n", x_y, y_y, z_y ); // Exhaustively flip the minus sign of the z axis until we find the right one . . . FLT bestErr = 9999.0; @@ -359,7 +360,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); FLT cx,cy,cz; CrossProduct(cx,cy,cz,x2[0][0],x_y,x2[2][0],y2[0][0],y_y,y2[2][0]); FLT hand = cx*z2[0][0] + cy*z_y + cz*z2[2][0]; - printf("err %f hand %f\n", err, hand); + //printf("err %f hand %f\n", err, hand); // If we are the best right-handed frame so far if (hand > 0 && err < bestErr) { x2[1][0]=x_y; y2[1][0]=y_y; z2[1][0]=z_y; bestErr=err; } @@ -380,7 +381,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); } } ydist = bestYdist; - +*/ /* for (i=0; itimebase_hz / 384000; //125 @ 48 MHz. int base_station = is_new_pulse; - printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); + //printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length ); } } -- cgit v1.2.3 From 472d05c2a356ec6d71669d67fb599e401a6f4a76 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Thu, 23 Mar 2017 00:25:25 -0700 Subject: Updated disambiguator. Solid OOTX --- calibrate.c | 4 +- data_recorder.c | 4 +- include/libsurvive/survive.h | 3 +- src/survive_cal.c | 9 +- src/survive_data.c | 211 +++++++++++++++++++++++++++++++++++++++---- src/survive_process.c | 6 +- 6 files changed, 208 insertions(+), 29 deletions(-) diff --git a/calibrate.c b/calibrate.c index 726a8cc..0f2d6ac 100644 --- a/calibrate.c +++ b/calibrate.c @@ -51,10 +51,10 @@ int bufferpts[32*2*3][2]; char buffermts[32*128*3]; int buffertimeto[32*3][2]; -void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) +void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh) { // if( timeinsweep < 0 ) return; - survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length ); + survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh); if( sensor_id < 0 ) return; if( acode == -1 ) return; //return; diff --git a/data_recorder.c b/data_recorder.c index 46f3427..6432f4f 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -43,9 +43,9 @@ int bufferpts[32*2*3]; char buffermts[32*128*3]; int buffertimeto[32*3]; -void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) +void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh) { - survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length ); + survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh); if( acode == -1 ) return; //return; diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index e04586c..c3b6a03 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -47,6 +47,7 @@ struct SurviveObject int32_t pulse_synctime_slack; //5,000 for normal vive hardware. (guessed) //Flood info, for calculating which laser is currently sweeping. + void * disambiguator_data; int8_t oldcode; int8_t sync_set_number; //0 = master, 1 = slave, -1 = fault. int8_t did_handle_ootx; //If unset, will send lightcap data for sync pulses next time a sensor is hit. @@ -129,7 +130,7 @@ void survive_cal_install( SurviveContext * ctx ); //XXX This will be removed if //Call these from your callback if overridden. //Accept higher-level data. -void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); +void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length , int lh); void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id ); void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); diff --git a/src/survive_cal.c b/src/survive_cal.c index c36a48a..51d12dd 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -190,7 +190,7 @@ void survive_cal_install( struct SurviveContext * ctx ) } -void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) +void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh) { struct SurviveContext * ctx = so->ctx; struct SurviveCalData * cd = ctx->calptr; @@ -210,7 +210,6 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int { int lhid = -sensor_id-1; // Take the OOTX data from the first device. - // TODO: Improve this so we'll watch all devices looking for OOTX data. Why limit ourselves to just one? if( lhid < NUM_LIGHTHOUSES && so == cd->poseobjects[0] ) { uint8_t dbit = (acode & 2)>>1; @@ -219,9 +218,9 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int int i; for( i = 0; i < NUM_LIGHTHOUSES; i++ ) if( ctx->bsd[i].OOTXSet == 0 ) break; - //if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //If all lighthouses have their OOTX set, move on. <------- Revert This!!!!! - if( i == 1 ) - cd->stage = 2; //If all lighthouses have their OOTX set, move on. + if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //If all lighthouses have their OOTX set, move on. <------- Revert This!!!!! + //if( i == 1 ) + //cd->stage = 2; //If all lighthouses have their OOTX set, move on. } break; } diff --git a/src/survive_data.c b/src/survive_data.c index e6538d1..2ef4940 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -5,7 +5,32 @@ #include #include -int getAcodeFromSyncPulse(int pulseLen) +typedef struct +{ + unsigned int sweep_time[SENSORS_PER_OBJECT]; + unsigned int sweep_len[SENSORS_PER_OBJECT]; +} lightcaps_sweep_data; +typedef struct +{ + lightcaps_sweep_data sweep; +} lightcap2_data; + +typedef struct +{ + int recent_sync_time; + int activeLighthouse; + int activeSweepStartTime; + int activeAcode; + + int lh_pulse_len[NUM_LIGHTHOUSES]; + int lh_start_time[NUM_LIGHTHOUSES]; + int current_lh; // used knowing which sync pulse we're looking at. + +} lightcap2_global_data; + +static lightcap2_global_data lcgd = { 0 }; + +int handle_lightcap2_getAcodeFromSyncPulse(int pulseLen) { if (pulseLen < 3125) return 0; if (pulseLen < 3625) return 1; @@ -16,48 +41,201 @@ int getAcodeFromSyncPulse(int pulseLen) if (pulseLen < 6125) return 6; return 7; } +void handle_lightcap2_process_sweep_data(SurviveObject *so) +{ + lightcap2_data *lcd = so->disambiguator_data; + + // look at all of the sensors we found, and process the ones that were hit. + // TODO: find the sensor(s) with the longest pulse length, and assume + // those are the "highest quality". Then, reject any pulses that are sufficiently + // different from those values, assuming that they are reflections. + { + unsigned int longest_pulse = 0; + unsigned int timestamp_of_longest_pulse = 0; + for (int i = 0; i < SENSORS_PER_OBJECT; i++) + { + if (lcd->sweep.sweep_len[i] > longest_pulse) + { + longest_pulse = lcd->sweep.sweep_len[i]; + timestamp_of_longest_pulse = lcd->sweep.sweep_time[i]; + } + } + + for (int i = 0; i < SENSORS_PER_OBJECT; i++) + { + if (lcd->sweep.sweep_len[i] != 0) // if the sensor was hit, process it + { + int offset_from = lcd->sweep.sweep_time[i] - lcgd.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2; + + if (offset_from < 380000 && offset_from > 70000) + { + int timeDelta = abs(timestamp_of_longest_pulse - lcd->sweep.sweep_time[i]); + if (timeDelta < 15000) // if this sweep point is within ~7 degrees of the point with the longest pulse. + { + so->ctx->lightproc(so, i, lcgd.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcgd.activeLighthouse); + } + } + } + } + } + // clear out sweep data (could probably limit this to only after a "first" sync. + // this is slightly more robust, so doing it here for now. + memset(&(((lightcap2_data*)so->disambiguator_data)->sweep), 0, sizeof(lightcaps_sweep_data)); +} void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) { - fprintf(stderr, "%d\n", le->length); + //fprintf(stderr, "%6.6d %4.4d \n", le->timestamp - so->recent_sync_time, le->length); + lightcap2_data *lcd = so->disambiguator_data; - if (le->timestamp - so->recent_sync_time < 24000) + //static unsigned int recent_sync_time = 0; + //static unsigned int recent_sync_count = -1; + //static unsigned int activeSweepStartTime; + + + // Process any sweep data we have + handle_lightcap2_process_sweep_data(so); + + int time_since_last_sync = (le->timestamp - lcgd.recent_sync_time); + + fprintf(stderr, " %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length); + // need to store up sync pulses, so we can take the earliest starting time for all sensors. + if (time_since_last_sync < 2400) { + lcgd.recent_sync_time = le->timestamp; + // it's the same sync pulse; + so->sync_set_number = 1; + //so->ctx->lightproc(so, recent_sync_count, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right. + so->recent_sync_time = le->timestamp; + + lcgd.lh_pulse_len[lcgd.current_lh] = le->length; + lcgd.lh_start_time[lcgd.current_lh] = le->timestamp; + + int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); + if (!(acode >> 2 & 1)) // if the skip bit is not set + { + lcgd.activeLighthouse = lcgd.current_lh; + lcgd.activeSweepStartTime = le->timestamp; + lcgd.activeAcode = acode; + } + else + { + lcgd.activeLighthouse = -1; + lcgd.activeSweepStartTime = 0; + lcgd.activeAcode = 0; + } + } + else if (time_since_last_sync < 24000) + { + //recent_sync_count--; + lcgd.recent_sync_time = le->timestamp; // I do believe we are lighthouse B - so->last_sync_time[1] = so->recent_sync_time = le->timestamp; - so->last_sync_length[1] = le->length; - if (le->length < 4625) // max non-skip pulse + 1/4 the difference in time between pulses. + lcgd.current_lh = 1; + lcgd.lh_pulse_len[lcgd.current_lh] = le->length; + lcgd.lh_start_time[lcgd.current_lh] = le->timestamp; + + int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); + + //{ + //fprintf(stderr, "2"); + //so->ctx->lightproc(so, -2, acode, le->length, le->timestamp, le->length); // Don't think I got this quite right. + + + //} + if (!(acode >> 2 & 1)) // if the skip bit is not set { - so->sync_set_number = 1; - so->ctx->lightproc(so, -2, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right. + if (lcgd.activeLighthouse != -1) + { + // hmm, it appears we got two non-skip pulses at the same time. That should never happen + fprintf(stderr, "WARNING: Two non-skip pulses received on the same cycle!\n"); + } + lcgd.activeLighthouse = 1; + lcgd.activeSweepStartTime = le->timestamp; + lcgd.activeAcode = acode; } + } - else + else if (time_since_last_sync > 370000) { // looks like this is the first sync pulse. Cool! - if (le->length < 4625) // max non-skip pulse + 1/4 the difference in time between pulses. + + // first, send out the sync pulse data for the last round (for OOTX decoding { - so->sync_set_number = 1; - so->ctx->lightproc(so, -1, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right. - } - } + if (lcgd.lh_pulse_len[0] != 0) + { + so->ctx->lightproc( + so, + -1, + handle_lightcap2_getAcodeFromSyncPulse(lcgd.lh_pulse_len[0]), + lcgd.lh_pulse_len[0], + lcgd.lh_start_time[0], + 0, + 0); + } + if (lcgd.lh_pulse_len[1] != 0) + { + so->ctx->lightproc( + so, + -2, + handle_lightcap2_getAcodeFromSyncPulse(lcgd.lh_pulse_len[1]), + lcgd.lh_pulse_len[1], + lcgd.lh_start_time[1], + 0, + 1); + } + } + // initialize here. + memset(&lcgd, 0, sizeof(lcgd)); + lcgd.activeLighthouse = -1; -// ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1] ); -// typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); + lcgd.recent_sync_time = le->timestamp; + // I do believe we are lighthouse A + lcgd.current_lh = 0; + lcgd.lh_pulse_len[lcgd.current_lh] = le->length; + lcgd.lh_start_time[lcgd.current_lh] = le->timestamp; + + int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); + //{ + // so->ctx->lightproc(so, -1, acode, le->length, le->timestamp, le->length); // Don't think I got this quite right. + //} + + if (!(acode >> 2 & 1)) // if the skip bit is not set + { + lcgd.activeLighthouse = 0; + lcgd.activeSweepStartTime = le->timestamp; + lcgd.activeAcode = acode; + } + + } } void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) { + lightcap2_data *lcd = so->disambiguator_data; + // If we see multiple "hits" on the sweep for a given sensor, + // assume that the longest (i.e. strongest signal) is most likely + // the non-reflected signal. + if (lcd->sweep.sweep_len[le->sensor_id] < le->length) + { + lcd->sweep.sweep_len[le->sensor_id] = le->length; + lcd->sweep.sweep_time[le->sensor_id] = le->timestamp; + } } void handle_lightcap2( SurviveObject * so, LightcapElement * le ) { SurviveContext * ctx = so->ctx; + if (so->disambiguator_data == NULL) + { + so->disambiguator_data = malloc(sizeof(lightcap2_data)); + memset(so->disambiguator_data, 0, sizeof(lightcap2_data)); + } + if( le->sensor_id > SENSORS_PER_OBJECT ) { return; @@ -72,6 +250,7 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * le ) { // Looks like a sync pulse, process it! handle_lightcap2_sync(so, le); + return; } // must be a sweep pulse, process it! diff --git a/src/survive_process.c b/src/survive_process.c index 8dc849a..6735f10 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -6,14 +6,14 @@ //XXX TODO: Once data is avialble in the context, use the stuff here to handle converting from time codes to //proper angles, then from there perform the rest of the solution. -void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) +void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh) { SurviveContext * ctx = so->ctx; - int base_station = acode >> 2; + int base_station = lh; int axis = acode & 1; if( ctx->calptr ) { - survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length ); + survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length, lh); } if( base_station > NUM_LIGHTHOUSES ) return; -- cgit v1.2.3 From 7701d14440d5d005b1ec3513ff8646e2f6b101ef Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Thu, 23 Mar 2017 08:11:36 -0700 Subject: Cleanup --- src/survive_data.c | 27 +++++++++++---------------- 1 file changed, 11 insertions(+), 16 deletions(-) diff --git a/src/survive_data.c b/src/survive_data.c index 2ef4940..d92db36 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -69,8 +69,7 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) if (offset_from < 380000 && offset_from > 70000) { - int timeDelta = abs(timestamp_of_longest_pulse - lcd->sweep.sweep_time[i]); - if (timeDelta < 15000) // if this sweep point is within ~7 degrees of the point with the longest pulse. + if (longest_pulse *10 / 8 < -lcd->sweep.sweep_len[i]) { so->ctx->lightproc(so, i, lcgd.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcgd.activeLighthouse); } @@ -97,14 +96,13 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) int time_since_last_sync = (le->timestamp - lcgd.recent_sync_time); - fprintf(stderr, " %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length); + //fprintf(stderr, " %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length); // need to store up sync pulses, so we can take the earliest starting time for all sensors. if (time_since_last_sync < 2400) { lcgd.recent_sync_time = le->timestamp; // it's the same sync pulse; so->sync_set_number = 1; - //so->ctx->lightproc(so, recent_sync_count, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right. so->recent_sync_time = le->timestamp; lcgd.lh_pulse_len[lcgd.current_lh] = le->length; @@ -126,7 +124,6 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) } else if (time_since_last_sync < 24000) { - //recent_sync_count--; lcgd.recent_sync_time = le->timestamp; // I do believe we are lighthouse B lcgd.current_lh = 1; @@ -135,12 +132,6 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); - //{ - //fprintf(stderr, "2"); - //so->ctx->lightproc(so, -2, acode, le->length, le->timestamp, le->length); // Don't think I got this quite right. - - - //} if (!(acode >> 2 & 1)) // if the skip bit is not set { if (lcgd.activeLighthouse != -1) @@ -197,9 +188,6 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) lcgd.lh_start_time[lcgd.current_lh] = le->timestamp; int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); - //{ - // so->ctx->lightproc(so, -1, acode, le->length, le->timestamp, le->length); // Don't think I got this quite right. - //} if (!(acode >> 2 & 1)) // if the skip bit is not set { @@ -207,9 +195,7 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) lcgd.activeSweepStartTime = le->timestamp; lcgd.activeAcode = acode; } - } - } void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) @@ -219,6 +205,15 @@ void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) // If we see multiple "hits" on the sweep for a given sensor, // assume that the longest (i.e. strongest signal) is most likely // the non-reflected signal. + + if (le->length < 80) + { + // this is a low-quality read. Better to throw it out than to use it. + //fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); + return; + } + fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); + if (lcd->sweep.sweep_len[le->sensor_id] < le->length) { lcd->sweep.sweep_len[le->sensor_id] = le->length; -- cgit v1.2.3 From 280a6599fea76a7d2c16cfe0fcc5c8f37fde66de Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 23 Mar 2017 09:47:38 -0700 Subject: More cleanup --- calibrate.c | 2 +- data_recorder.c | 2 +- include/libsurvive/survive.h | 2 +- include/libsurvive/survive_types.h | 2 +- src/survive_cal.c | 2 +- src/survive_cal.h | 2 +- src/survive_data.c | 96 +++++++++++++++++++------------------- src/survive_process.c | 2 +- src/survive_vive.c | 2 +- 9 files changed, 57 insertions(+), 55 deletions(-) diff --git a/calibrate.c b/calibrate.c index 0f2d6ac..82869e0 100644 --- a/calibrate.c +++ b/calibrate.c @@ -51,7 +51,7 @@ int bufferpts[32*2*3][2]; char buffermts[32*128*3]; int buffertimeto[32*3][2]; -void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh) +void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh) { // if( timeinsweep < 0 ) return; survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh); diff --git a/data_recorder.c b/data_recorder.c index 6432f4f..04a219a 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -43,7 +43,7 @@ int bufferpts[32*2*3]; char buffermts[32*128*3]; int buffertimeto[32*3]; -void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh) +void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh) { survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh); diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index c3b6a03..1165e9d 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -130,7 +130,7 @@ void survive_cal_install( SurviveContext * ctx ); //XXX This will be removed if //Call these from your callback if overridden. //Accept higher-level data. -void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length , int lh); +void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length , uint32_t lh); void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id ); void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index 1600e11..def30b8 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -28,7 +28,7 @@ typedef struct BaseStationData BaseStationData; typedef struct SurviveCalData SurviveCalData; //XXX Warning: This may be removed. Check at a later time for its defunctness. typedef void (*text_feedback_func)( SurviveContext * ctx, const char * fault ); -typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); +typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse); typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id ); typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); diff --git a/src/survive_cal.c b/src/survive_cal.c index 51d12dd..dfa2e85 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -190,7 +190,7 @@ void survive_cal_install( struct SurviveContext * ctx ) } -void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh) +void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh) { struct SurviveContext * ctx = so->ctx; struct SurviveCalData * cd = ctx->calptr; diff --git a/src/survive_cal.h b/src/survive_cal.h index 8c488bd..9d7b3a9 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -29,7 +29,7 @@ int survive_cal_get_status( SurviveContext * ctx, char * description, int descri //void survive_cal_teardown( struct SurviveContext * ctx ); //Called from survive_default_light_process -void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); +void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse); void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); #define MAX_SENSORS_TO_CAL 96 diff --git a/src/survive_data.c b/src/survive_data.c index d92db36..00e66f0 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -10,11 +10,6 @@ typedef struct unsigned int sweep_time[SENSORS_PER_OBJECT]; unsigned int sweep_len[SENSORS_PER_OBJECT]; } lightcaps_sweep_data; -typedef struct -{ - lightcaps_sweep_data sweep; -} lightcap2_data; - typedef struct { int recent_sync_time; @@ -28,7 +23,14 @@ typedef struct } lightcap2_global_data; -static lightcap2_global_data lcgd = { 0 }; +typedef struct +{ + lightcaps_sweep_data sweep; + lightcap2_global_data global; +} lightcap2_data; + + +//static lightcap2_global_data lcgd = { 0 }; int handle_lightcap2_getAcodeFromSyncPulse(int pulseLen) { @@ -65,13 +67,13 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) { if (lcd->sweep.sweep_len[i] != 0) // if the sensor was hit, process it { - int offset_from = lcd->sweep.sweep_time[i] - lcgd.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2; + int offset_from = lcd->sweep.sweep_time[i] - lcd->global.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2; if (offset_from < 380000 && offset_from > 70000) { - if (longest_pulse *10 / 8 < -lcd->sweep.sweep_len[i]) + if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i]) { - so->ctx->lightproc(so, i, lcgd.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcgd.activeLighthouse); + so->ctx->lightproc(so, i, lcd->global.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcd->global.activeLighthouse); } } } @@ -94,54 +96,54 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) // Process any sweep data we have handle_lightcap2_process_sweep_data(so); - int time_since_last_sync = (le->timestamp - lcgd.recent_sync_time); + int time_since_last_sync = (le->timestamp - lcd->global.recent_sync_time); //fprintf(stderr, " %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length); // need to store up sync pulses, so we can take the earliest starting time for all sensors. if (time_since_last_sync < 2400) { - lcgd.recent_sync_time = le->timestamp; + lcd->global.recent_sync_time = le->timestamp; // it's the same sync pulse; so->sync_set_number = 1; so->recent_sync_time = le->timestamp; - lcgd.lh_pulse_len[lcgd.current_lh] = le->length; - lcgd.lh_start_time[lcgd.current_lh] = le->timestamp; + lcd->global.lh_pulse_len[lcd->global.current_lh] = le->length; + lcd->global.lh_start_time[lcd->global.current_lh] = le->timestamp; int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); if (!(acode >> 2 & 1)) // if the skip bit is not set { - lcgd.activeLighthouse = lcgd.current_lh; - lcgd.activeSweepStartTime = le->timestamp; - lcgd.activeAcode = acode; + lcd->global.activeLighthouse = lcd->global.current_lh; + lcd->global.activeSweepStartTime = le->timestamp; + lcd->global.activeAcode = acode; } else { - lcgd.activeLighthouse = -1; - lcgd.activeSweepStartTime = 0; - lcgd.activeAcode = 0; + lcd->global.activeLighthouse = -1; + lcd->global.activeSweepStartTime = 0; + lcd->global.activeAcode = 0; } } else if (time_since_last_sync < 24000) { - lcgd.recent_sync_time = le->timestamp; + lcd->global.recent_sync_time = le->timestamp; // I do believe we are lighthouse B - lcgd.current_lh = 1; - lcgd.lh_pulse_len[lcgd.current_lh] = le->length; - lcgd.lh_start_time[lcgd.current_lh] = le->timestamp; + lcd->global.current_lh = 1; + lcd->global.lh_pulse_len[lcd->global.current_lh] = le->length; + lcd->global.lh_start_time[lcd->global.current_lh] = le->timestamp; int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); if (!(acode >> 2 & 1)) // if the skip bit is not set { - if (lcgd.activeLighthouse != -1) + if (lcd->global.activeLighthouse != -1) { // hmm, it appears we got two non-skip pulses at the same time. That should never happen fprintf(stderr, "WARNING: Two non-skip pulses received on the same cycle!\n"); } - lcgd.activeLighthouse = 1; - lcgd.activeSweepStartTime = le->timestamp; - lcgd.activeAcode = acode; + lcd->global.activeLighthouse = 1; + lcd->global.activeSweepStartTime = le->timestamp; + lcd->global.activeAcode = acode; } } @@ -151,49 +153,49 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) // first, send out the sync pulse data for the last round (for OOTX decoding { - if (lcgd.lh_pulse_len[0] != 0) + if (lcd->global.lh_pulse_len[0] != 0) { so->ctx->lightproc( so, -1, - handle_lightcap2_getAcodeFromSyncPulse(lcgd.lh_pulse_len[0]), - lcgd.lh_pulse_len[0], - lcgd.lh_start_time[0], + handle_lightcap2_getAcodeFromSyncPulse(lcd->global.lh_pulse_len[0]), + lcd->global.lh_pulse_len[0], + lcd->global.lh_start_time[0], 0, 0); } - if (lcgd.lh_pulse_len[1] != 0) + if (lcd->global.lh_pulse_len[1] != 0) { so->ctx->lightproc( so, -2, - handle_lightcap2_getAcodeFromSyncPulse(lcgd.lh_pulse_len[1]), - lcgd.lh_pulse_len[1], - lcgd.lh_start_time[1], + handle_lightcap2_getAcodeFromSyncPulse(lcd->global.lh_pulse_len[1]), + lcd->global.lh_pulse_len[1], + lcd->global.lh_start_time[1], 0, 1); } } // initialize here. - memset(&lcgd, 0, sizeof(lcgd)); - lcgd.activeLighthouse = -1; + memset(&lcd->global, 0, sizeof(lcd->global)); + lcd->global.activeLighthouse = -1; - lcgd.recent_sync_time = le->timestamp; + lcd->global.recent_sync_time = le->timestamp; // I do believe we are lighthouse A - lcgd.current_lh = 0; - lcgd.lh_pulse_len[lcgd.current_lh] = le->length; - lcgd.lh_start_time[lcgd.current_lh] = le->timestamp; + lcd->global.current_lh = 0; + lcd->global.lh_pulse_len[lcd->global.current_lh] = le->length; + lcd->global.lh_start_time[lcd->global.current_lh] = le->timestamp; int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); if (!(acode >> 2 & 1)) // if the skip bit is not set { - lcgd.activeLighthouse = 0; - lcgd.activeSweepStartTime = le->timestamp; - lcgd.activeAcode = acode; + lcd->global.activeLighthouse = 0; + lcd->global.activeSweepStartTime = le->timestamp; + lcd->global.activeAcode = acode; } } } @@ -383,8 +385,8 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) int32_t delta1 = so->last_sync_time[0] - so->recent_sync_time; int32_t delta2 = so->last_sync_time[1] - so->last_sync_time[0]; - ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0] ); - ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1] ); + ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0 ); + ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1 ); so->recent_sync_time = so->last_sync_time[1]; @@ -427,7 +429,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) //Make sure pulse is in valid window if( offset_from < 380000 && offset_from > 70000 ) { - ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length ); + ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, so->sync_set_number ); } } else diff --git a/src/survive_process.c b/src/survive_process.c index 6735f10..463481a 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -6,7 +6,7 @@ //XXX TODO: Once data is avialble in the context, use the stuff here to handle converting from time codes to //proper angles, then from there perform the rest of the solution. -void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh) +void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh) { SurviveContext * ctx = so->ctx; int base_station = lh; diff --git a/src/survive_vive.c b/src/survive_vive.c index a9b295f..c04fc03 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -141,7 +141,7 @@ void survive_data_cb( SurviveUSBInterface * si ); //USB Subsystem void survive_usb_close( SurviveContext * t ); -int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1, SurviveObject * tr0 ); +int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1, SurviveObject * tr0, struct SurviveObject * ww0 ); int survive_usb_poll( SurviveContext * ctx ); int survive_get_config( char ** config, SurviveViveData * ctx, int devno, int iface, int send_extra_magic ); int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen ); -- cgit v1.2.3 From f33018188bf5bdf6f6b8af0d646e3f8c519d9d71 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 23 Mar 2017 12:13:44 -0700 Subject: Added support for empty string in config.json & other cleanup. --- redist/json_helpers.c | 3 ++- src/survive_cal.c | 50 +++++++++++++++++++++----------------------------- src/survive_config.c | 10 +++++++++- src/survive_data.c | 2 +- 4 files changed, 33 insertions(+), 32 deletions(-) diff --git a/redist/json_helpers.c b/redist/json_helpers.c index 0267932..3b5cc0d 100644 --- a/redist/json_helpers.c +++ b/redist/json_helpers.c @@ -117,7 +117,8 @@ char* load_file_to_mem(const char* path) { fseek( f, 0, SEEK_END ); int len = ftell( f ); fseek( f, 0, SEEK_SET ); - char * JSON_STRING = malloc( len ); + char * JSON_STRING = malloc( len + 1); + memset(JSON_STRING,0,len+1); fread( JSON_STRING, len, 1, f ); fclose( f ); return JSON_STRING; diff --git a/src/survive_cal.c b/src/survive_cal.c index 19eb3ca..f9ec595 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -124,50 +124,42 @@ void survive_cal_install( struct SurviveContext * ctx ) cd->numPoseObjects = 0; - for (int i=0; i < ctx->objs_ct; i++) + const char * RequiredTrackersForCal = config_read_str( ctx->global_config_values, "RequiredTrackersForCal", "HMD,WM0,WM1" ); + const uint32_t AllowAllTrackersForCal = config_read_uint32( ctx->global_config_values, "AllowAllTrackersForCal", 0 ); + size_t requiredTrackersFound = 0; + + for (int j=0; j < ctx->objs_ct; j++) { - // This would be a place where we could conditionally decide if we - // want to include a certain device in the calibration routine. - if (1) + // Add the tracker if we allow all trackers for calibration, or if it's in the list + // of required trackers. + int isRequiredTracker = strstr(RequiredTrackersForCal, ctx->objs[j]->codename) != NULL; + + if (isRequiredTracker) { - cd->poseobjects[i] = ctx->objs[i]; + requiredTrackersFound++; + } + + if (AllowAllTrackersForCal || isRequiredTracker) + { + cd->poseobjects[j] = ctx->objs[j]; cd->numPoseObjects++; - SV_INFO("Calibration is using %s", cd->poseobjects[i]->codename); + SV_INFO("Calibration is using %s", cd->poseobjects[j]->codename); } + } // If we want to mandate that certain devices have been found - - - //cd->poseobjects[0] = survive_get_so_by_name( ctx, "HMD" ); - //cd->poseobjects[1] = survive_get_so_by_name( ctx, "WM0" ); - //cd->poseobjects[2] = survive_get_so_by_name( ctx, "WM1" ); - - //if( cd->poseobjects[0] == 0 || cd->poseobjects[1] == 0 || cd->poseobjects[2] == 0 ) - //{ - // SV_ERROR( "Error: cannot find all devices needed for calibration." ); - // free( cd ); - // return; - //} - -//XXX TODO MWTourney, work on your code here. -/* - if( !cd->hmd ) + if (strlen(RequiredTrackersForCal) > 0) { - cd->hmd = survive_get_so_by_name( ctx, "TR0" ); - - if( !cd->hmd ) + if (requiredTrackersFound != ((strlen(RequiredTrackersForCal) + 1) / 4)) { - SV_ERROR( "Error: cannot find any devices labeled HMD. Required for calibration" ); + SV_ERROR( "Error: cannot find all devices needed for calibration." ); free( cd ); return; } - SV_INFO( "HMD not found, calibrating using Tracker" ); } -*/ - const char * DriverName; const char * PreferredPoser = config_read_str( ctx->global_config_values, "ConfigPoser", "PoserCharlesSlow" ); diff --git a/src/survive_config.c b/src/survive_config.c index 0810280..005cfaf 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -175,7 +175,13 @@ const char* config_set_str(config_group *cg, const char *tag, const char* value) if (cv == NULL) cv = next_unused_entry(cg); sstrcpy(&(cv->tag), tag); - sstrcpy(&(cv->data), value); + + if (NULL != value){ + sstrcpy(&(cv->data), value); + } + else { + sstrcpy(&(cv->data), ""); + } cv->type = CONFIG_STRING; return value; @@ -357,9 +363,11 @@ void handle_tag_value(char* tag, char** values, uint8_t count) { print_json_value(tag,values,count); config_group* cg = cg_stack[cg_stack_head]; + if (NULL != *values){ if (parse_uint32(tag,values,count) > 0) return; //parse integers first, stricter rules if (parse_floats(tag,values,count) > 0) return; + } //should probably also handle string arrays config_set_str(cg,tag,values[0]); diff --git a/src/survive_data.c b/src/survive_data.c index ee180b1..da4e0a2 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -372,7 +372,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz. int base_station = is_new_pulse; //printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); - ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length ); + ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length, base_station); } } -- cgit v1.2.3 From 3d86dda66d50d1e1955d4e0cd5f374e0aed1789f Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 23 Mar 2017 16:26:27 -0700 Subject: Fixed Windows USB Interface Enumeration --- src/survive_cal.c | 4 ++-- src/survive_data.c | 21 +++++++++++---------- src/survive_vive.c | 18 ++++++++++++++---- 3 files changed, 27 insertions(+), 16 deletions(-) diff --git a/src/survive_cal.c b/src/survive_cal.c index f9ec595..81889ca 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -155,7 +155,7 @@ void survive_cal_install( struct SurviveContext * ctx ) { if (requiredTrackersFound != ((strlen(RequiredTrackersForCal) + 1) / 4)) { - SV_ERROR( "Error: cannot find all devices needed for calibration." ); + SV_ERROR( "Error: Did not find all devices required for calibration." ); free( cd ); return; } @@ -201,7 +201,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int if( sensor_id < 0 ) { int lhid = -sensor_id-1; - // Take the OOTX data from the first device. + // Take the OOTX data from the first device. (if using HMD, WM0, WM1 only, this will be HMD) if( lhid < NUM_LIGHTHOUSES && so == cd->poseobjects[0] ) { uint8_t dbit = (acode & 2)>>1; diff --git a/src/survive_data.c b/src/survive_data.c index da4e0a2..ce263ed 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -208,13 +208,14 @@ void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) // assume that the longest (i.e. strongest signal) is most likely // the non-reflected signal. - if (le->length < 80) - { - // this is a low-quality read. Better to throw it out than to use it. - //fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); - return; - } - fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); + //if (le->length < 80) + //{ + // // this is a low-quality read. Better to throw it out than to use it. + // //fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); + // return; + //} + //fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); + //fprintf(stderr, "."); if (lcd->sweep.sweep_len[le->sensor_id] < le->length) { @@ -269,8 +270,8 @@ int32_t decode_acode(uint32_t length, int32_t main_divisor) { void handle_lightcap( SurviveObject * so, LightcapElement * le ) { SurviveContext * ctx = so->ctx; -// handle_lightcap2(so,le); -// return; + handle_lightcap2(so,le); + return; //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; @@ -373,7 +374,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) int base_station = is_new_pulse; //printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length, base_station); - } + } } diff --git a/src/survive_vive.c b/src/survive_vive.c index f6465b2..bfe4926 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -340,7 +340,7 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * if (cur_dev->vendor_id == vendor_id && cur_dev->product_id == product_id) { - if( menum == enumid ) + if( cur_dev->interface_number == enumid ) { path_to_open = cur_dev->path; break; @@ -521,12 +521,21 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo if (sv->udev[USB_DEV_W_WATCHMAN1]) { + //static uint8_t vive_magic_power_on[64] = { 0x04, 0x78, 0x29, 0x38 }; + static uint8_t vive_magic_power_on[5] = { 0x04 }; + //static uint8_t vive_magic_power_on[5] = { 0x04, 0x00, 0xc3, 0xe2, 0x04 }; + r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); + if( r != sizeof( vive_magic_power_on ) ) return 5; + } + if (sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP]) + { + //static uint8_t vive_magic_enable_lighthouse[64] = { 0x04, 0x78, 0x29, 0x38 }; static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; - r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); + r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates) - r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); + r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; } @@ -534,7 +543,8 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo for( int i = 0; i < 256; i++ ) { static uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0xff, 0, 0, 0, 0, 0, 0, 0 }; - r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); + //r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); + r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); SV_INFO( "UCR: %d", r ); if( r != sizeof( vive_controller_haptic_pulse ) ) return 5; OGUSleep( 1000 ); -- cgit v1.2.3 From c55c5928485da0432140be4befe5165c68ae4ab0 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 23 Mar 2017 16:46:04 -0700 Subject: Change disambiguator filter --- src/survive_data.c | 2 +- src/survive_vive.c | 45 +++++++++++++++++++++++++++++++++------------ 2 files changed, 34 insertions(+), 13 deletions(-) diff --git a/src/survive_data.c b/src/survive_data.c index ce263ed..849b722 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -71,7 +71,7 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) if (offset_from < 380000 && offset_from > 70000) { - if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i]) + //if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i]) { so->ctx->lightproc(so, i, lcd->global.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcd->global.activeLighthouse); } diff --git a/src/survive_vive.c b/src/survive_vive.c index bfe4926..f58fb79 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -47,6 +47,7 @@ const short vidpids[] = { 0x28de, 0x2012, 0, //Valve Watchman, USB connected #ifdef HIDAPI 0x28de, 0x2000, 1, //Valve HMD lighthouse(B) (only used on HIDAPI, for lightcap) + 0x28de, 0x2022, 1, //HTC Tracker (only used on HIDAPI, for lightcap) 0x28de, 0x2012, 1, //Valve Watchman, USB connected (only used on HIDAPI, for lightcap) #endif }; //length MAX_USB_INTERFACES*2 @@ -60,6 +61,7 @@ const char * devnames[] = { "Wired Watchman 1", #ifdef HIDAPI "HMD Lightcap", + "Tracker 0 Lightcap", "Wired Watchman 1 Lightcap", #endif }; //length MAX_USB_INTERFACES @@ -74,8 +76,9 @@ const char * devnames[] = { #ifdef HIDAPI #define USB_DEV_LIGHTHOUSEB 6 -#define USB_DEV_W_WATCHMAN1_LIGHTCAP 7 -#define MAX_USB_DEVS 8 +#define USB_DEV_TRACKER0_LIGHTCAP 7 +#define USB_DEV_W_WATCHMAN1_LIGHTCAP 8 +#define MAX_USB_DEVS 9 #else #define MAX_USB_DEVS 6 #endif @@ -87,8 +90,9 @@ const char * devnames[] = { #define USB_IF_TRACKER0 4 #define USB_IF_W_WATCHMAN1 5 #define USB_IF_LIGHTCAP 6 -#define USB_IF_W_WATCHMAN1_LIGHTCAP 7 -#define MAX_INTERFACES 8 +#define USB_IF_TRACKER0_LIGHTCAP 7 +#define USB_IF_W_WATCHMAN1_LIGHTCAP 8 +#define MAX_INTERFACES 9 typedef struct SurviveUSBInterface SurviveUSBInterface; typedef struct SurviveViveData SurviveViveData; @@ -475,9 +479,11 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * #ifdef HIDAPI //Tricky: use other interface for actual lightcap. XXX THIS IS NOT YET RIGHT!!! if( sv->udev[USB_DEV_LIGHTHOUSEB] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_LIGHTHOUSEB], 0x82, survive_data_cb, "Lightcap")) { return -12; } + if( sv->udev[USB_DEV_TRACKER0_LIGHTCAP] && AttachInterface( sv, tr0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0x82, survive_data_cb, "Tracker 1 Lightcap")) { return -13; } if( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; } #else if( sv->udev[USB_DEV_LIGHTHOUSE] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_LIGHTHOUSE], 0x82, survive_data_cb, "Lightcap")) { return -12; } + if( sv->udev[USB_DEV_TRACKER0] && AttachInterface( sv, ww0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0], 0x82, survive_data_cb, "Tracker 0 Lightcap")) { return -13; } if( sv->udev[USB_DEV_W_WATCHMAN1] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; } #endif SV_INFO( "All enumerated devices attached." ); @@ -521,9 +527,7 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo if (sv->udev[USB_DEV_W_WATCHMAN1]) { - //static uint8_t vive_magic_power_on[64] = { 0x04, 0x78, 0x29, 0x38 }; static uint8_t vive_magic_power_on[5] = { 0x04 }; - //static uint8_t vive_magic_power_on[5] = { 0x04, 0x00, 0xc3, 0xe2, 0x04 }; r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); if( r != sizeof( vive_magic_power_on ) ) return 5; } @@ -539,6 +543,23 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; } + if (sv->udev[USB_DEV_TRACKER0]) + { + static uint8_t vive_magic_power_on[5] = { 0x04 }; + r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); + if( r != sizeof( vive_magic_power_on ) ) return 5; + } + if (sv->udev[USB_DEV_TRACKER0_LIGHTCAP]) + { + static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; + r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); + if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; + + static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates) + r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); + if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; + } + #if 0 for( int i = 0; i < 256; i++ ) { @@ -551,12 +572,12 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo } #endif - if (sv->udev[USB_DEV_TRACKER0]) - { - static uint8_t vive_magic_power_on[64] = { 0x04, 0x78, 0x29, 0x38 }; - r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); - if( r != sizeof( vive_magic_power_on ) ) return 5; - } + //if (sv->udev[USB_DEV_TRACKER0]) + //{ + // static uint8_t vive_magic_power_on[64] = { 0x04, 0x78, 0x29, 0x38 }; + // r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); + // if( r != sizeof( vive_magic_power_on ) ) return 5; + //} SV_INFO( "Powered unit on." ); } -- cgit v1.2.3 From 5404526ae8da8c5fdff81b8ee8120ffe73647747 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Fri, 24 Mar 2017 01:05:07 -0400 Subject: Dave's affine solve is getting close. --- dave/AffineSolve | Bin 40104 -> 40104 bytes redist/linmath.c | 2 +- src/poser_daveortho.c | 68 +++++++++++++++++++++++++++++++++++++------------- 3 files changed, 51 insertions(+), 19 deletions(-) diff --git a/dave/AffineSolve b/dave/AffineSolve index 7d51b34..bd93cbd 100755 Binary files a/dave/AffineSolve and b/dave/AffineSolve differ diff --git a/redist/linmath.c b/redist/linmath.c index eefcd5f..1724a13 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -215,7 +215,7 @@ void quatfrommatrix( FLT * q, const FLT * matrix44 ) q[1] = (matrix44[9] - matrix44[6]) / S; q[2] = (matrix44[2] - matrix44[8]) / S; q[3] = (matrix44[4] - matrix44[1]) / S; - } else if ((matrix44[0] > matrix44[5])&(matrix44[0] > matrix44[10])) { + } else if ((matrix44[0] > matrix44[5])&&(matrix44[0] > matrix44[10])) { FLT S = FLT_SQRT(1.0 + matrix44[0] - matrix44[5] - matrix44[10]) * 2.; // S=4*qx q[0] = (matrix44[9] - matrix44[6]) / S; q[1] = 0.25f * S; diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index 80f65a9..906b21e 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -456,41 +456,73 @@ PRINT(ab,2,1); //------------------- // Orthogonalize the matrix //------------------- - - PRINT_MAT(T,4,4); -matrix44transpose(T2, T); -//matrix44copy(T2,T); - cross3d( &T2[0][0], &T2[1][0], &T2[2][0] ); - cross3d( &T2[2][0], &T2[0][0], &T2[1][0] ); - normalize3d( &T2[0][0], &T2[0][0] ); - normalize3d( &T2[1][0], &T2[1][0] ); - normalize3d( &T2[2][0], &T2[2][0] ); -//matrix44copy(T2,T); -matrix44transpose(T, T2); PRINT_MAT(T,4,4); +#if 1 +// matrix44transpose(T2, T); //Transpose so we are + matrix44copy(T2,T); + cross3d( &T2[1][0], &T2[0][0], &T2[2][0] ); + cross3d( &T2[2][0], &T2[1][0], &T2[0][0] ); //Replace axes in-place. + matrix44copy(T,T2); +// matrix44transpose(T, T2); + +#endif + + normalize3d( &T[0][0], &T[0][0] ); + normalize3d( &T[1][0], &T[1][0] ); + normalize3d( &T[2][0], &T[2][0] ); + //Change handedness + + T[1][0]*=-1; + T[1][1]*=-1; + T[1][2]*=-1; + +/* + //Check Orthogonality. Yep. It's orthogonal. + FLT tmp[3]; + cross3d( tmp, &T[0][0], &T[1][0] ); + printf( "M3: %f\n", magnitude3d( tmp ) ); + cross3d( tmp, &T[2][0], &T[1][0] ); + printf( "M3: %f\n", magnitude3d( tmp ) ); + cross3d( tmp, &T[2][0], &T[0][0] ); + printf( "M3: %f\n", magnitude3d( tmp ) ); +*/ + +// PRINT_MAT(T,4,4); +#if 1 - //memcpy(T2,T,16*sizeof(float)); -// matrix44copy(T2,T); + //matrix44copy(T2,T); matrix44transpose(T2,T); + quatfrommatrix( quat, &T2[0][0] ); + printf( "QM: %f\n", quatmagnitude( quat ) ); quatnormalize(quatNorm,quat); quattoeuler(euler,quatNorm); - quattomatrix( T2, quatNorm ); + quattomatrix( &T2[0][0], quatNorm ); + + PRINT_MAT(T2,4,4); printf("rot %f %f %f len %f\n", euler[0], euler[1], euler[2], quatmagnitude(quat)); - PRINT(T,4,4); +// PRINT(T,4,4); // matrix44copy(temp,T2); matrix44transpose(temp,T2); - memcpy(T2,temp,16*sizeof(float)); + +// matrix44transpose(T2, temp); +// memcpy(T2,temp,16*sizeof(float)); for (i=0; i<3; i++) { for (j=0; j<3; j++) { - T[i][j] = T2[j][i]; + T[i][j] = temp[i][j]; } } - PRINT(T2,4,4); + +/* PRINT(T2,4,4); */ +#endif + + T[1][0]*=-1; + T[1][1]*=-1; + T[1][2]*=-1; /* -- cgit v1.2.3 From 9f6c7e87266c5a7d721d4d8e3ace82dc46207387 Mon Sep 17 00:00:00 2001 From: Michael Turvey Date: Fri, 24 Mar 2017 10:10:14 -0700 Subject: Fix linux build Successfully tested with a wired watchman on linux. Wired tracker now succeeds in providing calibration info Still not getting light info from tracker. --- calibrate_client.c | 11 +++++++---- src/survive_vive.c | 31 ++++++++++++++++++++++++++++++- 2 files changed, 37 insertions(+), 5 deletions(-) diff --git a/calibrate_client.c b/calibrate_client.c index 1a2ee41..1e18321 100644 --- a/calibrate_client.c +++ b/calibrate_client.c @@ -43,9 +43,9 @@ int bufferpts[32*2*3]; char buffermts[32*128*3]; int buffertimeto[32*3]; -void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) +void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh) { - survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length ); + survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh); if( acode == -1 ) return; //return; @@ -160,7 +160,7 @@ int main() // config_save("config.json"); */ - + ctx = survive_init( 1 ); survive_install_light_fn( ctx, my_light_process ); @@ -219,9 +219,12 @@ int main() so = wm0; if( strcmp( dev, "WM1" ) == 0 ) so = wm1; + uint32_t lh = 0; + if (lineptr[0] == 'r') + lh = 1; if( so ) - my_light_process( so, sensor_id, acode, timeinsweep, timecode, length ); + my_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh ); break; } diff --git a/src/survive_vive.c b/src/survive_vive.c index f58fb79..c1bcb26 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -531,9 +531,10 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); if( r != sizeof( vive_magic_power_on ) ) return 5; } + +#ifdef HIDAPI if (sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP]) { - //static uint8_t vive_magic_enable_lighthouse[64] = { 0x04, 0x78, 0x29, 0x38 }; static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; @@ -543,12 +544,27 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; } +#else + if (sv->udev[USB_DEV_W_WATCHMAN1]) + { + static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; + r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); + if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; + + static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates) + r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); + if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; + } + +#endif + if (sv->udev[USB_DEV_TRACKER0]) { static uint8_t vive_magic_power_on[5] = { 0x04 }; r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); if( r != sizeof( vive_magic_power_on ) ) return 5; } +#ifdef HIDAPI if (sv->udev[USB_DEV_TRACKER0_LIGHTCAP]) { static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; @@ -559,6 +575,19 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; } +#else + if (sv->udev[USB_DEV_TRACKER0]) + { + static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; + r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); + if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; + + static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates) + r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); + if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; + } + +#endif #if 0 for( int i = 0; i < 256; i++ ) -- cgit v1.2.3 From a576703242adea11c012af5afdff38af84d22e2e Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 24 Mar 2017 13:44:38 -0700 Subject: Adaptive Acode Offset Compensation --- calibrate.c | 2 +- src/survive_cal.c | 5 +-- src/survive_data.c | 114 +++++++++++++++++++++++++++++++---------------------- src/survive_vive.c | 30 ++++++++------ 4 files changed, 86 insertions(+), 65 deletions(-) diff --git a/calibrate.c b/calibrate.c index 62acbf6..6e2b5d1 100644 --- a/calibrate.c +++ b/calibrate.c @@ -59,7 +59,7 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int if( acode < 0 ) return; //return; int jumpoffset = sensor_id; - if( strcmp( so->codename, "WM0" ) == 0 ) jumpoffset += 32; + if( strcmp( so->codename, "WM0" ) == 0 || strcmp( so->codename, "WW0" ) == 0) jumpoffset += 32; else if( strcmp( so->codename, "WM1" ) == 0 ) jumpoffset += 64; diff --git a/src/survive_cal.c b/src/survive_cal.c index 81889ca..22a8eff 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -200,6 +200,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int //Collecting OOTX data. if( sensor_id < 0 ) { + //fprintf(stderr, "%s\n", so->codename); int lhid = -sensor_id-1; // Take the OOTX data from the first device. (if using HMD, WM0, WM1 only, this will be HMD) if( lhid < NUM_LIGHTHOUSES && so == cd->poseobjects[0] ) @@ -210,9 +211,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int int i; for( i = 0; i < NUM_LIGHTHOUSES; i++ ) if( ctx->bsd[i].OOTXSet == 0 ) break; - if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //If all lighthouses have their OOTX set, move on. <------- Revert This!!!!! - //if( i == 1 ) - //cd->stage = 2; //If all lighthouses have their OOTX set, move on. + if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //TODO: Make this configuratble to allow single lighthouse. } break; case 3: //Look for light sync lengths. diff --git a/src/survive_data.c b/src/survive_data.c index 849b722..4e2479a 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -21,26 +21,42 @@ typedef struct int lh_start_time[NUM_LIGHTHOUSES]; int current_lh; // used knowing which sync pulse we're looking at. +} lightcap2_per_sweep_data; + +typedef struct +{ + float acode_offset; } lightcap2_global_data; typedef struct { lightcaps_sweep_data sweep; + lightcap2_per_sweep_data per_sweep; lightcap2_global_data global; } lightcap2_data; //static lightcap2_global_data lcgd = { 0 }; -int handle_lightcap2_getAcodeFromSyncPulse(int pulseLen) +int handle_lightcap2_getAcodeFromSyncPulse(SurviveObject * so, int pulseLen) { - if (pulseLen < 3125) return 0; - if (pulseLen < 3625) return 1; - if (pulseLen < 4125) return 2; - if (pulseLen < 4625) return 3; - if (pulseLen < 5125) return 4; - if (pulseLen < 5625) return 5; - if (pulseLen < 6125) return 6; + float oldOffset = ((lightcap2_data*)so->disambiguator_data)->global.acode_offset; + + int modifiedPulseLen = pulseLen - (int)oldOffset; + + float newOffset = (((pulseLen) + 250) % 500) - 250; + + ((lightcap2_data*)so->disambiguator_data)->global.acode_offset = oldOffset * 0.9 + newOffset * 0.1; + +//fprintf(stderr, " %f\n", oldOffset); +#define ACODE_OFFSET 0 + if (pulseLen < 3250 - ACODE_OFFSET) return 0; + if (pulseLen < 3750 - ACODE_OFFSET) return 1; + if (pulseLen < 4250 - ACODE_OFFSET) return 2; + if (pulseLen < 4750 - ACODE_OFFSET) return 3; + if (pulseLen < 5250 - ACODE_OFFSET) return 4; + if (pulseLen < 5750 - ACODE_OFFSET) return 5; + if (pulseLen < 6250 - ACODE_OFFSET) return 6; return 7; } void handle_lightcap2_process_sweep_data(SurviveObject *so) @@ -67,13 +83,13 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) { if (lcd->sweep.sweep_len[i] != 0) // if the sensor was hit, process it { - int offset_from = lcd->sweep.sweep_time[i] - lcd->global.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2; + int offset_from = lcd->sweep.sweep_time[i] - lcd->per_sweep.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2; if (offset_from < 380000 && offset_from > 70000) { //if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i]) { - so->ctx->lightproc(so, i, lcd->global.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcd->global.activeLighthouse); + so->ctx->lightproc(so, i, lcd->per_sweep.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcd->per_sweep.activeLighthouse); } } } @@ -96,54 +112,54 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) // Process any sweep data we have handle_lightcap2_process_sweep_data(so); - int time_since_last_sync = (le->timestamp - lcd->global.recent_sync_time); + int time_since_last_sync = (le->timestamp - lcd->per_sweep.recent_sync_time); //fprintf(stderr, " %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length); // need to store up sync pulses, so we can take the earliest starting time for all sensors. if (time_since_last_sync < 2400) { - lcd->global.recent_sync_time = le->timestamp; + lcd->per_sweep.recent_sync_time = le->timestamp; // it's the same sync pulse; so->sync_set_number = 1; so->recent_sync_time = le->timestamp; - lcd->global.lh_pulse_len[lcd->global.current_lh] = le->length; - lcd->global.lh_start_time[lcd->global.current_lh] = le->timestamp; + lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; + lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; - int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); + int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); if (!(acode >> 2 & 1)) // if the skip bit is not set { - lcd->global.activeLighthouse = lcd->global.current_lh; - lcd->global.activeSweepStartTime = le->timestamp; - lcd->global.activeAcode = acode; + lcd->per_sweep.activeLighthouse = lcd->per_sweep.current_lh; + lcd->per_sweep.activeSweepStartTime = le->timestamp; + lcd->per_sweep.activeAcode = acode; } else { - lcd->global.activeLighthouse = -1; - lcd->global.activeSweepStartTime = 0; - lcd->global.activeAcode = 0; + lcd->per_sweep.activeLighthouse = -1; + lcd->per_sweep.activeSweepStartTime = 0; + lcd->per_sweep.activeAcode = 0; } } else if (time_since_last_sync < 24000) { - lcd->global.recent_sync_time = le->timestamp; + lcd->per_sweep.recent_sync_time = le->timestamp; // I do believe we are lighthouse B - lcd->global.current_lh = 1; - lcd->global.lh_pulse_len[lcd->global.current_lh] = le->length; - lcd->global.lh_start_time[lcd->global.current_lh] = le->timestamp; + lcd->per_sweep.current_lh = 1; + lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; + lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; - int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); + int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); if (!(acode >> 2 & 1)) // if the skip bit is not set { - if (lcd->global.activeLighthouse != -1) + if (lcd->per_sweep.activeLighthouse != -1) { // hmm, it appears we got two non-skip pulses at the same time. That should never happen fprintf(stderr, "WARNING: Two non-skip pulses received on the same cycle!\n"); } - lcd->global.activeLighthouse = 1; - lcd->global.activeSweepStartTime = le->timestamp; - lcd->global.activeAcode = acode; + lcd->per_sweep.activeLighthouse = 1; + lcd->per_sweep.activeSweepStartTime = le->timestamp; + lcd->per_sweep.activeAcode = acode; } } @@ -153,49 +169,50 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) // first, send out the sync pulse data for the last round (for OOTX decoding { - if (lcd->global.lh_pulse_len[0] != 0) + if (lcd->per_sweep.lh_pulse_len[0] != 0) { so->ctx->lightproc( so, -1, - handle_lightcap2_getAcodeFromSyncPulse(lcd->global.lh_pulse_len[0]), - lcd->global.lh_pulse_len[0], - lcd->global.lh_start_time[0], + handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_pulse_len[0]), + lcd->per_sweep.lh_pulse_len[0], + lcd->per_sweep.lh_start_time[0], 0, 0); } - if (lcd->global.lh_pulse_len[1] != 0) + if (lcd->per_sweep.lh_pulse_len[1] != 0) { so->ctx->lightproc( so, -2, - handle_lightcap2_getAcodeFromSyncPulse(lcd->global.lh_pulse_len[1]), - lcd->global.lh_pulse_len[1], - lcd->global.lh_start_time[1], + handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_pulse_len[1]), + lcd->per_sweep.lh_pulse_len[1], + lcd->per_sweep.lh_start_time[1], 0, 1); } } + //fprintf(stderr, "************************************ Reinitializing Disambiguator!!!\n"); // initialize here. - memset(&lcd->global, 0, sizeof(lcd->global)); - lcd->global.activeLighthouse = -1; + memset(&lcd->per_sweep, 0, sizeof(lcd->per_sweep)); + lcd->per_sweep.activeLighthouse = -1; - lcd->global.recent_sync_time = le->timestamp; + lcd->per_sweep.recent_sync_time = le->timestamp; // I do believe we are lighthouse A - lcd->global.current_lh = 0; - lcd->global.lh_pulse_len[lcd->global.current_lh] = le->length; - lcd->global.lh_start_time[lcd->global.current_lh] = le->timestamp; + lcd->per_sweep.current_lh = 0; + lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; + lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; - int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); + int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); if (!(acode >> 2 & 1)) // if the skip bit is not set { - lcd->global.activeLighthouse = 0; - lcd->global.activeSweepStartTime = le->timestamp; - lcd->global.activeAcode = acode; + lcd->per_sweep.activeLighthouse = 0; + lcd->per_sweep.activeSweepStartTime = le->timestamp; + lcd->per_sweep.activeAcode = acode; } } } @@ -230,6 +247,7 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * le ) if (so->disambiguator_data == NULL) { + fprintf(stderr, "Initializing Disambiguator Data\n"); so->disambiguator_data = malloc(sizeof(lightcap2_data)); memset(so->disambiguator_data, 0, sizeof(lightcap2_data)); } diff --git a/src/survive_vive.c b/src/survive_vive.c index c1bcb26..55e949a 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -479,6 +479,9 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * #ifdef HIDAPI //Tricky: use other interface for actual lightcap. XXX THIS IS NOT YET RIGHT!!! if( sv->udev[USB_DEV_LIGHTHOUSEB] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_LIGHTHOUSEB], 0x82, survive_data_cb, "Lightcap")) { return -12; } + + // This is a HACK! But it works. Need to investigate further + sv->uiface[USB_DEV_TRACKER0_LIGHTCAP].actual_len = 64; if( sv->udev[USB_DEV_TRACKER0_LIGHTCAP] && AttachInterface( sv, tr0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0x82, survive_data_cb, "Tracker 1 Lightcap")) { return -13; } if( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; } #else @@ -564,18 +567,18 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); if( r != sizeof( vive_magic_power_on ) ) return 5; } -#ifdef HIDAPI - if (sv->udev[USB_DEV_TRACKER0_LIGHTCAP]) - { - static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; - r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); - if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; - - static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates) - r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); - if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; - } -#else +//#ifdef HIDAPI +// if (sv->udev[USB_DEV_TRACKER0_LIGHTCAP]) +// { +// static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; +// r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); +// if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; +// +// static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates) +// r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); +// if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; +// } +//#else if (sv->udev[USB_DEV_TRACKER0]) { static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; @@ -587,7 +590,7 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; } -#endif +//#endif #if 0 for( int i = 0; i < 256; i++ ) @@ -1186,6 +1189,7 @@ void survive_data_cb( SurviveUSBInterface * si ) break; } case USB_IF_W_WATCHMAN1_LIGHTCAP: + case USB_IF_TRACKER0_LIGHTCAP: { int i=0; for( i = 0; i < 7; i++ ) -- cgit v1.2.3 From f7d1c19e299df46087eb8693d2d0c0637d2e395f Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 24 Mar 2017 13:52:21 -0700 Subject: Fix Calibrate visualization to use proper lighthouse data --- calibrate.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/calibrate.c b/calibrate.c index 6e2b5d1..ee22abf 100644 --- a/calibrate.c +++ b/calibrate.c @@ -63,24 +63,24 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int else if( strcmp( so->codename, "WM1" ) == 0 ) jumpoffset += 64; - if( acode == 0 || acode == 2 ) //data = 0 + if( acode % 2 == 0 && lh == 0) //data = 0 { bufferpts[jumpoffset*2+0][0] = (timeinsweep-100000)/500; buffertimeto[jumpoffset][0] = 0; } - if( acode == 1 || acode == 3 ) //data = 1 + if( acode % 2 == 1 && lh == 0 ) //data = 1 { bufferpts[jumpoffset*2+1][0] = (timeinsweep-100000)/500; buffertimeto[jumpoffset][0] = 0; } - if( acode == 4 || acode == 6 ) //data = 0 + if( acode % 2 == 0 && lh == 1 ) //data = 0 { bufferpts[jumpoffset*2+0][1] = (timeinsweep-100000)/500; buffertimeto[jumpoffset][1] = 0; } - if( acode == 5 || acode == 7 ) //data = 1 + if( acode % 2 == 1 && lh == 1 ) //data = 1 { bufferpts[jumpoffset*2+1][1] = (timeinsweep-100000)/500; buffertimeto[jumpoffset][1] = 0; -- cgit v1.2.3 From 4dc1d72785c660c206f8def9d8c8aa32289c2709 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 24 Mar 2017 15:19:59 -0700 Subject: More cleanup & finishing genericization of calibrator --- calibrate.c | 4 ++-- include/libsurvive/survive.h | 2 +- include/libsurvive/survive_types.h | 2 +- redist/CNFGWinDriver.c | 2 +- redist/json_helpers.c | 2 +- src/survive_cal.c | 30 +++++++++++++++++++----------- src/survive_cal.h | 6 ++++-- src/survive_data.c | 6 +++--- src/survive_process.c | 6 +++--- src/survive_vive.c | 6 ++---- 10 files changed, 37 insertions(+), 29 deletions(-) diff --git a/calibrate.c b/calibrate.c index ee22abf..abf592a 100644 --- a/calibrate.c +++ b/calibrate.c @@ -99,9 +99,9 @@ void my_imu_process( struct SurviveObject * so, int mask, FLT * accelgyro, uint3 } -void my_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ) +void my_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh) { - survive_default_angle_process( so, sensor_id, acode, timecode, length, angle ); + survive_default_angle_process( so, sensor_id, acode, timecode, length, angle, lh ); } char* sensor_name[32]; diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 1165e9d..e13312d 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -132,7 +132,7 @@ void survive_cal_install( SurviveContext * ctx ); //XXX This will be removed if //Accept higher-level data. void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length , uint32_t lh); void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id ); -void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); +void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ); ////////////////////// Survive Drivers //////////////////////////// diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index def30b8..bfd0b1d 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -30,7 +30,7 @@ typedef struct SurviveCalData SurviveCalData; //XXX Warning: This may be remov typedef void (*text_feedback_func)( SurviveContext * ctx, const char * fault ); typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse); typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id ); -typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); +typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh); //Device drivers (prefix your drivers with "DriverReg") i.e. diff --git a/redist/CNFGWinDriver.c b/redist/CNFGWinDriver.c index c5da925..b1c1eb0 100644 --- a/redist/CNFGWinDriver.c +++ b/redist/CNFGWinDriver.c @@ -232,7 +232,7 @@ void CNFGHandleInput() case WM_MBUTTONUP: HandleButton( (msg.lParam & 0xFFFF), (msg.lParam>>16) & 0xFFFF, 3, 0 ); break; case WM_KEYDOWN: case WM_KEYUP: - HandleKey( tolower( msg.wParam ), (msg.message==WM_KEYDOWN) ); + HandleKey( tolower( (int)(msg.wParam) ), (msg.message==WM_KEYDOWN) ); break; default: DispatchMessage(&msg); diff --git a/redist/json_helpers.c b/redist/json_helpers.c index 3b5cc0d..29d48bd 100644 --- a/redist/json_helpers.c +++ b/redist/json_helpers.c @@ -174,7 +174,7 @@ void json_load_file(const char* path) { int16_t children = -1; - for (i=0; i<(int)items; i+=2) + for (i=0; i<(unsigned int)items; i+=2) { //increment i on each successful tag + values combination, not individual tokens jsmntok_t* tag_t = tokens+i; diff --git a/src/survive_cal.c b/src/survive_cal.c index 22a8eff..6c153b4 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -220,10 +220,13 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int else if( acode < -4 ) break; int lh = (-acode) - 3; - if( strcmp( so->codename, "WM0" ) == 0 ) - sensor_id += 32; - if( strcmp( so->codename, "WM1" ) == 0 ) - sensor_id += 64; + for (int i=0; i < min(MAX_DEVICES_TO_CAL, cd->numPoseObjects); i++) + { + if( strcmp( so->codename, cd->poseobjects[i]->codename ) == 0 ) + { + sensor_id += i*32; + } + } cd->all_sync_times[sensor_id][lh][cd->all_sync_counts[sensor_id][lh]++] = length; break; @@ -233,7 +236,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int } -void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ) +void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ) { struct SurviveContext * ctx = so->ctx; struct SurviveCalData * cd = ctx->calptr; @@ -241,14 +244,18 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin if( !cd ) return; int sensid = sensor_id; - if( strcmp( so->codename, "WM0" ) == 0 ) - sensid += 32; - if( strcmp( so->codename, "WM1" ) == 0 ) - sensid += 64; + + for (int i=0; i < min(MAX_DEVICES_TO_CAL, cd->numPoseObjects); i++) + { + if( strcmp( so->codename, cd->poseobjects[i]->codename ) == 0 ) + { + sensid += i*32; + } + } if( sensid >= MAX_SENSORS_TO_CAL || sensid < 0 ) return; - int lighthouse = acode>>2; + int lighthouse = lh; int axis = acode & 1; switch( cd->stage ) @@ -292,7 +299,8 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin int min_peaks = PTS_BEFORE_COMMON; int i, j, k; cd->found_common = 1; - for( i = 0; i < MAX_SENSORS_TO_CAL/SENSORS_PER_OBJECT; i++ ) + for( i = 0; i < cd->numPoseObjects; i++ ) + //for( i = 0; i < MAX_SENSORS_TO_CAL/SENSORS_PER_OBJECT; i++ ) for( j = 0; j < NUM_LIGHTHOUSES; j++ ) { int sensors_visible = 0; diff --git a/src/survive_cal.h b/src/survive_cal.h index 8f4e4de..ae644d1 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -30,9 +30,11 @@ int survive_cal_get_status( SurviveContext * ctx, char * description, int descri //Called from survive_default_light_process void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse); -void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); +void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ); -#define MAX_SENSORS_TO_CAL 96 +#define MAX_SENSORS_PER_DEVICE 32 +#define MAX_DEVICES_TO_CAL 3 +#define MAX_SENSORS_TO_CAL (MAX_SENSORS_PER_DEVICE * MAX_DEVICES_TO_CAL) #define MIN_PTS_BEFORE_CAL 24 diff --git a/src/survive_data.c b/src/survive_data.c index 4e2479a..9447104 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -25,7 +25,7 @@ typedef struct typedef struct { - float acode_offset; + double acode_offset; } lightcap2_global_data; typedef struct @@ -40,11 +40,11 @@ typedef struct int handle_lightcap2_getAcodeFromSyncPulse(SurviveObject * so, int pulseLen) { - float oldOffset = ((lightcap2_data*)so->disambiguator_data)->global.acode_offset; + double oldOffset = ((lightcap2_data*)so->disambiguator_data)->global.acode_offset; int modifiedPulseLen = pulseLen - (int)oldOffset; - float newOffset = (((pulseLen) + 250) % 500) - 250; + double newOffset = (((pulseLen) + 250) % 500) - 250; ((lightcap2_data*)so->disambiguator_data)->global.acode_offset = oldOffset * 0.9 + newOffset * 0.1; diff --git a/src/survive_process.c b/src/survive_process.c index d4604d8..b58b344 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -37,16 +37,16 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode #endif FLT length_sec = length / (FLT)so->timebase_hz; - ctx->angleproc( so, sensor_id, acode, timecode, length_sec, angle ); + ctx->angleproc( so, sensor_id, acode, timecode, length_sec, angle, lh); } -void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ) +void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh) { SurviveContext * ctx = so->ctx; if( ctx->calptr ) { - survive_cal_angle( so, sensor_id, acode, timecode, length, angle ); + survive_cal_angle( so, sensor_id, acode, timecode, length, angle, lh ); } if( so->PoserFn ) { diff --git a/src/survive_vive.c b/src/survive_vive.c index 55e949a..a5c731d 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -834,7 +834,6 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) qty-=2; int propset = 0; int doimu = 0; - int i; if( (type & 0xf0) == 0xf0 ) { @@ -911,10 +910,9 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) *readdata = type; //Put 'type' back on stack. uint8_t * mptr = readdata + qty-3-1; //-3 for timecode, -1 to -//#define DEBUG_WATCHMAN #ifdef DEBUG_WATCHMAN printf( "_%s ", w->codename); - for( i = 0; i < qty; i++ ) + for(int i = 0; i < qty; i++ ) { printf( "%02x ", readdata[i] ); } @@ -1198,7 +1196,7 @@ void survive_data_cb( SurviveUSBInterface * si ) unsigned short *length = (unsigned short *)(&(readdata[2])); unsigned long *time = (unsigned long *)(&(readdata[4])); LightcapElement le; - le.sensor_id = POP2; + le.sensor_id = (uint8_t)POP2; le.length = POP2; le.timestamp = POP4; if( le.sensor_id == 0xff ) break; -- cgit v1.2.3 From a279a202768e1cae10f4fb651aad2b3b21db43ab Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 24 Mar 2017 15:36:43 -0700 Subject: remove min() --- src/survive_cal.c | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/src/survive_cal.c b/src/survive_cal.c index 6c153b4..5414048 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -141,10 +141,17 @@ void survive_cal_install( struct SurviveContext * ctx ) if (AllowAllTrackersForCal || isRequiredTracker) { - cd->poseobjects[j] = ctx->objs[j]; - cd->numPoseObjects++; + if (MAX_DEVICES_TO_CAL < cd->numPoseObjects) + { + cd->poseobjects[j] = ctx->objs[j]; + cd->numPoseObjects++; - SV_INFO("Calibration is using %s", cd->poseobjects[j]->codename); + SV_INFO("Calibration is using %s", cd->poseobjects[j]->codename); + } + else + { + SV_ERROR("Calibration is NOT using %s; device count exceeds MAX_DEVICES_TO_CAL", cd->poseobjects[j]->codename); + } } } @@ -220,7 +227,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int else if( acode < -4 ) break; int lh = (-acode) - 3; - for (int i=0; i < min(MAX_DEVICES_TO_CAL, cd->numPoseObjects); i++) + for (int i=0; i < cd->numPoseObjects; i++) { if( strcmp( so->codename, cd->poseobjects[i]->codename ) == 0 ) { @@ -234,6 +241,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int } + } void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ) @@ -245,7 +253,7 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin int sensid = sensor_id; - for (int i=0; i < min(MAX_DEVICES_TO_CAL, cd->numPoseObjects); i++) + for (int i=0; i < cd->numPoseObjects; i++) { if( strcmp( so->codename, cd->poseobjects[i]->codename ) == 0 ) { -- cgit v1.2.3 From efe1b3e98866d45dd3bb5f8060712db152762edd Mon Sep 17 00:00:00 2001 From: Michael Turvey Date: Fri, 24 Mar 2017 16:00:20 -0700 Subject: Fixing linux --- calibrate_client.c | 4 ++-- src/survive_cal.c | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/calibrate_client.c b/calibrate_client.c index 1e18321..abfbabc 100644 --- a/calibrate_client.c +++ b/calibrate_client.c @@ -90,9 +90,9 @@ void my_imu_process( struct SurviveObject * so, int mask, FLT * accelgyro, uint3 } -void my_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ) +void my_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ) { - survive_default_angle_process( so, sensor_id, acode, timecode, length, angle ); + survive_default_angle_process( so, sensor_id, acode, timecode, length, angle, lh ); } diff --git a/src/survive_cal.c b/src/survive_cal.c index 5414048..ae92bad 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -141,7 +141,7 @@ void survive_cal_install( struct SurviveContext * ctx ) if (AllowAllTrackersForCal || isRequiredTracker) { - if (MAX_DEVICES_TO_CAL < cd->numPoseObjects) + if (MAX_DEVICES_TO_CAL > cd->numPoseObjects) { cd->poseobjects[j] = ctx->objs[j]; cd->numPoseObjects++; @@ -150,7 +150,7 @@ void survive_cal_install( struct SurviveContext * ctx ) } else { - SV_ERROR("Calibration is NOT using %s; device count exceeds MAX_DEVICES_TO_CAL", cd->poseobjects[j]->codename); + SV_INFO("Calibration is NOT using %s; device count exceeds MAX_DEVICES_TO_CAL", ctx->objs[j]->codename); } } -- cgit v1.2.3 From ce7fa5b134b1394f7bb9f4bddb1da9e83d792827 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Sat, 25 Mar 2017 21:58:51 -0700 Subject: Adding Tori Poser --- include/libsurvive/survive.h | 6 +- src/poser_turveytori.c | 871 +++++++++++++++++++++++++ winbuild/libsurvive/libsurvive.vcxproj | 1 + winbuild/libsurvive/libsurvive.vcxproj.filters | 3 + 4 files changed, 878 insertions(+), 3 deletions(-) create mode 100644 src/poser_turveytori.c diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index e13312d..278bbca 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -32,9 +32,9 @@ struct SurviveObject PoserCB PoserFn; //Device-specific information about the location of the sensors. This data will be used by the poser. - int8_t nr_locations; - FLT * sensor_locations; - FLT * sensor_normals; + int8_t nr_locations; // sensor count + FLT * sensor_locations; // size is nr_locations*3. Contains x,y,z values for each sensor + FLT * sensor_normals;// size is nrlocations*3. cointains normal vector for each sensor //Timing sensitive data (mostly for disambiguation) int32_t timebase_hz; //48,000,000 for normal vive hardware. (checked) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c new file mode 100644 index 0000000..e9e5b7a --- /dev/null +++ b/src/poser_turveytori.c @@ -0,0 +1,871 @@ +#include +#include +#include +#include +#include +#include +#include "linmath.h" +#include +#include +#include + + +#define PointToFlts(x) ((FLT*)(x)) + +typedef struct +{ + FLT x; + FLT y; + FLT z; +} Point; + +void writePoint(FILE *file, double x, double y, double z, unsigned int rgb) {} +void updateHeader(FILE * file) {} +void writeAxes(FILE * file) {} +void drawLineBetweenPoints(FILE *file, Point a, Point b, unsigned int color) {} +void writePcdHeader(FILE * file) {} +void writePointCloud(FILE *f, Point *pointCloud, unsigned int Color) {} +void markPointWithStar(FILE *file, Point point, unsigned int color) {} + +typedef struct +{ + Point point; // location of the sensor on the tracked object; + Point normal; // unit vector indicating the normal for the sensor + double theta; // "horizontal" angular measurement from lighthouse radians + double phi; // "vertical" angular measurement from lighthouse in radians. +} TrackedSensor; + +typedef struct +{ + size_t numSensors; + TrackedSensor sensor[0]; +} TrackedObject; + + +#ifndef M_PI +#define M_PI 3.14159265358979323846264338327 +#endif + +#define SQUARED(x) ((x)*(x)) + +typedef union +{ + struct + { + unsigned char Blue; + unsigned char Green; + unsigned char Red; + unsigned char Alpha; + }; + uint32_t long_value; +} RGBValue; + +static RGBValue RED = { .Red = 255,.Green = 0,.Blue = 0,.Alpha = 125 }; +static RGBValue GREEN = { .Red = 0,.Green = 255,.Blue = 0,.Alpha = 125 }; +static RGBValue BLUE = { .Red = 0,.Green = 0,.Blue = 255,.Alpha = 125 }; + +static const double WORLD_BOUNDS = 100; +#define MAX_TRACKED_POINTS 40 + +static const float DefaultPointsPerOuterDiameter = 60; + +typedef struct +{ + int something; + //Stuff +} ToriData; + + + + + + + +static FLT distance(Point a, Point b) +{ + FLT x = a.x - b.x; + FLT y = a.y - b.y; + FLT z = a.z - b.z; + return FLT_SQRT(x*x + y*y + z*z); +} + +Matrix3x3 GetRotationMatrixForTorus(Point a, Point b) +{ + Matrix3x3 result; + FLT v1[3] = { 0, 0, 1 }; + FLT v2[3] = { a.x - b.x, a.y - b.y, a.z - b.z }; + + normalize3d(v2, v2); + + rotation_between_vecs_to_m3(&result, v1, v2); + + // Useful for debugging... + //FLT v2b[3]; + //rotate_vec(v2b, v1, result); + + return result; +} + +typedef struct +{ + Point a; + Point b; + FLT angle; + FLT tanAngle; // tangent of angle + Matrix3x3 rotation; + Matrix3x3 invRotation; // inverse of rotation + +} PointsAndAngle; + + +Point RotateAndTranslatePoint(Point p, Matrix3x3 rot, Point newOrigin) +{ + Point q; + + double pf[3] = { p.x, p.y, p.z }; + q.x = rot.val[0][0] * p.x + rot.val[1][0] * p.y + rot.val[2][0] * p.z + newOrigin.x; + q.y = rot.val[0][1] * p.x + rot.val[1][1] * p.y + rot.val[2][1] * p.z + newOrigin.y; + q.z = rot.val[0][2] * p.x + rot.val[1][2] * p.y + rot.val[2][2] * p.z + newOrigin.z; + + return q; +} + +double angleFromPoints(Point p1, Point p2, Point center) +{ + Point v1, v2, v1norm, v2norm; + v1.x = p1.x - center.x; + v1.y = p1.y - center.y; + v1.z = p1.z - center.z; + + v2.x = p2.x - center.x; + v2.y = p2.y - center.y; + v2.z = p2.z - center.z; + + double v1mag = sqrt(v1.x * v1.x + v1.y * v1.y + v1.z * v1.z); + v1norm.x = v1.x / v1mag; + v1norm.y = v1.y / v1mag; + v1norm.z = v1.z / v1mag; + + double v2mag = sqrt(v2.x * v2.x + v2.y * v2.y + v2.z * v2.z); + v2norm.x = v2.x / v2mag; + v2norm.y = v2.y / v2mag; + v2norm.z = v2.z / v2mag; + + double res = v1norm.x * v2norm.x + v1norm.y * v2norm.y + v1norm.z * v2norm.z; + + double angle = acos(res); + + return angle; +} + +Point midpoint(Point a, Point b) +{ + Point m; + m.x = (a.x + b.x) / 2; + m.y = (a.y + b.y) / 2; + m.z = (a.z + b.z) / 2; + + return m; +} + +// What we're doing here is: +// * Given a point in space +// * And points and a lighthouse angle that implicitly define a torus +// * for that torus, what is the toroidal angle of the plane that will go through that point in space +// * and given that toroidal angle, what is the poloidal angle that will be directed toward that point in space? +void estimateToroidalAndPoloidalAngleOfPoint( + PointsAndAngle *pna, + Point point, + double *toroidalSin, + double *toroidalCos, + double *poloidalAngle, + double *poloidalSin) +{ + // We take the inverse of the rotation matrix, and this now defines a rotation matrix that will take us from + // the tracked object coordinate system into the "easy" or "default" coordinate system of the torus. + // Using this will allow us to derive angles much more simply by being in a "friendly" coordinate system. + Matrix3x3 rot = pna->invRotation; + Point origin; + origin.x = 0; + origin.y = 0; + origin.z = 0; + + Point m = midpoint(pna->a, pna->b); + + // in this new coordinate system, we'll rename all of the points we care about to have an "F" after them + // This will be their representation in the "friendly" coordinate system + Point pointF; + + // Okay, I lied a little above. In addition to the rotation matrix that we care about, there was also + // a translation that we did to move the origin. If we're going to get to the "friendly" coordinate system + // of the torus, we need to first undo the translation, then undo the rotation. Below, we're undoing the translation. + pointF.x = point.x - m.x; + pointF.y = point.y - m.y; + pointF.z = point.z - m.z; + + // now we'll undo the rotation part. + pointF = RotateAndTranslatePoint(pointF, rot, origin); + + // hooray, now pointF is in our more-friendly coordinate system. + + // Now, it's time to figure out the toroidal angle to that point. This should be pretty easy. + // We will "flatten" the z dimension to only look at the x and y values. Then, we just need to measure the + // angle between a vector to pointF and a vector along the x axis. + + FLT toroidalHyp = FLT_SQRT(SQUARED(pointF.y) + SQUARED(pointF.x)); + + *toroidalSin = pointF.y / toroidalHyp; + + *toroidalCos = pointF.x / toroidalHyp; + + //*toroidalAngle = atan(pointF.y / pointF.x); + //if (pointF.x < 0) + //{ + // *toroidalAngle += M_PI; + //} + + //assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 < 0.000001); + //assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 > -0.000001); + + //assert(*toroidalCos / FLT_COS(*toroidalAngle) - 1 < 0.000001); + //assert(*toroidalCos / FLT_COS(*toroidalAngle) - 1 > -0.000001); + + // SCORE!! We've got the toroidal angle. We're half done! + + // Okay, what next...? Now, we will need to rotate the torus *again* to make it easy to + // figure out the poloidal angle. We should rotate the entire torus by the toroidal angle + // so that the point we're focusin on will lie on the x/z plane. We then should translate the + // torus so that the center of the poloidal circle is at the origin. At that point, it will + // be trivial to determine the poloidal angle-- it will be the angle on the xz plane of a + // vector from the origin to the point. + + // okay, instead of rotating the torus & point by the toroidal angle to get the point on + // the xz plane, we're going to take advantage of the radial symmetry of the torus + // (i.e. it's symmetric about the point we'd want to rotate it, so the rotation wouldn't + // change the torus at all). Therefore, we'll leave the torus as is, but we'll rotate the point + // This will only impact the x and y coordinates, and we'll use "G" as the postfix to represent + // this new coordinate system + + Point pointG; + pointG.z = pointF.z; + pointG.y = 0; + pointG.x = sqrt(SQUARED(pointF.x) + SQUARED(pointF.y)); + + // okay, that ended up being easier than I expected. Now that we have the point on the xZ plane, + // our next step will be to shift it down so that the center of the poloidal circle is at the origin. + // As you may have noticed, y has now gone to zero, and from here on out, we can basically treat + // this as a 2D problem. I think we're getting close... + + // I stole these lines from the torus generator. Gonna need the poloidal radius. + double distanceBetweenPoints = distance(pna->a, pna->b); // we don't care about the coordinate system of these points because we're just getting distance. + double toroidalRadius = distanceBetweenPoints / (2 * pna->tanAngle); + double poloidalRadius = sqrt(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2)); + + // The center of the polidal circle already lies on the z axis at this point, so we won't shift z at all. + // The shift along the X axis will be the toroidal radius. + + Point pointH; + pointH.z = pointG.z; + pointH.y = pointG.y; + pointH.x = pointG.x - toroidalRadius; + + // Okay, almost there. If we treat pointH as a vector on the XZ plane, if we get its angle, + // that will be the poloidal angle we're looking for. (crosses fingers) + + FLT poloidalHyp = FLT_SQRT(SQUARED(pointH.z) + SQUARED(pointH.x)); + + *poloidalSin = pointH.z / poloidalHyp; + + + *poloidalAngle = atan(pointH.z / pointH.x); + if (pointH.x < 0) + { + *poloidalAngle += M_PI; + } + + //assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 < 0.000001); + //assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 > -0.000001); + + + + // Wow, that ended up being not so much code, but a lot of interesting trig. + // can't remember the last time I spent so much time working through each line of code. + + return; +} + +#define MAX_POINT_PAIRS 100 + +FLT angleBetweenSensors(TrackedSensor *a, TrackedSensor *b) +{ + FLT angle = FLT_ACOS(FLT_COS(a->phi - b->phi)*FLT_COS(a->theta - b->theta)); + FLT angle2 = FLT_ACOS(FLT_COS(b->phi - a->phi)*FLT_COS(b->theta - a->theta)); + + return angle; +} + +// This provides a pretty good estimate of the angle above, probably better +// the further away the lighthouse is. But, it's not crazy-precise. +// It's main advantage is speed. +FLT pythAngleBetweenSensors2(TrackedSensor *a, TrackedSensor *b) +{ + FLT p = (a->phi - b->phi); + FLT d = (a->theta - b->theta); + + FLT adjd = FLT_SIN((a->phi + b->phi) / 2); + FLT adjP = FLT_SIN((a->theta + b->theta) / 2); + FLT pythAngle = sqrt(SQUARED(p*adjP) + SQUARED(d*adjd)); + return pythAngle; +} + +Point calculateTorusPointFromAngles(PointsAndAngle *pna, FLT toroidalSin, FLT toroidalCos, FLT poloidalAngle, FLT poloidalSin) +{ + Point result; + + FLT distanceBetweenPoints = distance(pna->a, pna->b); + Point m = midpoint(pna->a, pna->b); + Matrix3x3 rot = pna->rotation; + + FLT toroidalRadius = distanceBetweenPoints / (2 * pna->tanAngle); + FLT poloidalRadius = FLT_SQRT(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2)); + + result.x = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*toroidalCos; + result.y = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*toroidalSin; + result.z = poloidalRadius*poloidalSin; + result = RotateAndTranslatePoint(result, rot, m); + + return result; +} + +FLT getPointFitnessForPna(Point pointIn, PointsAndAngle *pna) +{ + + double toroidalSin = 0; + double toroidalCos = 0; + double poloidalAngle = 0; + double poloidalSin = 0; + + estimateToroidalAndPoloidalAngleOfPoint( + pna, + pointIn, + &toroidalSin, + &toroidalCos, + &poloidalAngle, + &poloidalSin); + + Point torusPoint = calculateTorusPointFromAngles(pna, toroidalSin, toroidalCos, poloidalAngle, poloidalSin); + + FLT dist = distance(pointIn, torusPoint); + + // This is some voodoo black magic. This is here to solve the problem that the origin + // (which is near the center of all the tori) erroniously will rank as a good match. + // through a lot of empiracle testing on how to compensate for this, the "fudge factor" + // below ended up being the best fit. As simple as it is, I have a strong suspicion + // that there's some crazy complex thesis-level math that could be used to derive this + // but it works so we'll run with it. + // Note that this may be resulting in a skewing of the found location by several millimeters. + // it is not clear if this is actually removing existing skew (to get a more accurate value) + // or if it is introducing an undesirable skew. + double fudge = FLT_SIN((poloidalAngle - M_PI) / 2); + dist = dist / fudge; + + return dist; +} + +FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount) +{ + FLT fitness; + + FLT resultSum = 0; + + for (size_t i = 0; i < pnaCount; i++) + { + fitness = getPointFitnessForPna(pointIn, &(pna[i])); + resultSum += SQUARED(fitness); + } + + return 1 / FLT_SQRT(resultSum); +} + +Point getGradient(Point pointIn, PointsAndAngle *pna, size_t pnaCount, FLT precision) +{ + Point result; + + Point tmpXplus = pointIn; + Point tmpXminus = pointIn; + tmpXplus.x = pointIn.x + precision; + tmpXminus.x = pointIn.x - precision; + result.x = getPointFitness(tmpXplus, pna, pnaCount) - getPointFitness(tmpXminus, pna, pnaCount); + + Point tmpYplus = pointIn; + Point tmpYminus = pointIn; + tmpYplus.y = pointIn.y + precision; + tmpYminus.y = pointIn.y - precision; + result.y = getPointFitness(tmpYplus, pna, pnaCount) - getPointFitness(tmpYminus, pna, pnaCount); + + Point tmpZplus = pointIn; + Point tmpZminus = pointIn; + tmpZplus.z = pointIn.z + precision; + tmpZminus.z = pointIn.z - precision; + result.z = getPointFitness(tmpZplus, pna, pnaCount) - getPointFitness(tmpZminus, pna, pnaCount); + + return result; +} + +Point getNormalizedVector(Point vectorIn, FLT desiredMagnitude) +{ + FLT distanceIn = sqrt(SQUARED(vectorIn.x) + SQUARED(vectorIn.y) + SQUARED(vectorIn.z)); + + FLT scale = desiredMagnitude / distanceIn; + + Point result = vectorIn; + + result.x *= scale; + result.y *= scale; + result.z *= scale; + + return result; +} + +Point getAvgPoints(Point a, Point b) +{ + Point result; + result.x = (a.x + b.x) / 2; + result.y = (a.y + b.y) / 2; + result.z = (a.z + b.z) / 2; + return result; +} + + +// This is modifies the basic gradient descent algorithm to better handle the shallow valley case, +// which appears to be typical of this convergence. +static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, FILE *logFile) +{ + int i = 0; + FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount); + 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; +} + + +// 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]; + + volatile size_t sizeNeeded = sizeof(pna); + + Point avgNorm = { 0 }; + + size_t pnaCount = 0; + for (unsigned int i = 0; i < obj->numSensors; i++) + { + for (unsigned int j = 0; j < i; j++) + { + if (pnaCount < MAX_POINT_PAIRS) + { + pna[pnaCount].a = obj->sensor[i].point; + pna[pnaCount].b = obj->sensor[j].point; + + pna[pnaCount].angle = angleBetweenSensors(&obj->sensor[i], &obj->sensor[j]); + //pna[pnaCount].angle = pythAngleBetweenSensors2(&obj->sensor[i], &obj->sensor[j]); + pna[pnaCount].tanAngle = FLT_TAN(pna[pnaCount].angle); + + double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta)); + + pna[pnaCount].rotation = GetRotationMatrixForTorus(pna[pnaCount].a, pna[pnaCount].b); + pna[pnaCount].invRotation = inverseM33(pna[pnaCount].rotation); + + + pnaCount++; + } + } + + avgNorm.x += obj->sensor[i].normal.x; + avgNorm.y += obj->sensor[i].normal.y; + avgNorm.z += obj->sensor[i].normal.z; + } + avgNorm.x = avgNorm.x / obj->numSensors; + avgNorm.y = avgNorm.y / obj->numSensors; + avgNorm.z = avgNorm.z / obj->numSensors; + + FLT avgNormF[3] = { avgNorm.x, avgNorm.y, avgNorm.z }; + + + FILE *logFile = NULL; + if (doLogOutput) + { + logFile = fopen("pointcloud2.pcd", "wb"); + writePcdHeader(logFile); + writeAxes(logFile); + } + + + // Point refinedEstimageGd = RefineEstimateUsingModifiedGradientDescent1(initialEstimate, pna, pnaCount, logFile); + + + // arbitrarily picking a value of 8 meters out to start from. + // intentionally picking the direction of the average normal vector of the sensors that see the lighthouse + // since this is least likely to pick the incorrect "mirror" point that would send us + // back into the search for the correct point (see "if (a1 > M_PI / 2)" below) + Point p1 = getNormalizedVector(avgNorm, 8); + + Point refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p1, pna, pnaCount, logFile); + + FLT pf1[3] = { refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z }; + + FLT a1 = anglebetween3d(pf1, avgNormF); + + if (a1 > M_PI / 2) + { + Point p2 = { .x = -refinedEstimateGd.x,.y = -refinedEstimateGd.y,.z = -refinedEstimateGd.z }; + refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p2, pna, pnaCount, logFile); + + //FLT pf2[3] = { refinedEstimageGd2.x, refinedEstimageGd2.y, refinedEstimageGd2.z }; + + //FLT a2 = anglebetween3d(pf2, avgNormF); + + } + + FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount); + + FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); + printf("(%4.4f, %4.4f, %4.4f)\n", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z); + printf("Distance is %f, Fitness is %f\n", distance, fitGd); + + if (logFile) + { + updateHeader(logFile); + fclose(logFile); + } + //fgetc(stdin); + return refinedEstimateGd; +} + + + + + + + + + + + + +int PoserTurveyTori( SurviveObject * so, PoserData * pd ) +{ + PoserType pt = pd->pt; + SurviveContext * ctx = so->ctx; + ToriData * dd = so->PoserData; + + if (!dd) + { + so->PoserData = dd = malloc(sizeof(ToriData)); + memset(dd, 0, sizeof(ToriData)); + } + + 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: + { + TrackedObject *to; + + PoserDataFullScene * fs = (PoserDataFullScene*)pd; + + to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor))); + + //FLT lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; + //FLT angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; //2 Axes (Angles in LH space) + //FLT synctimes[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES]; + + //to->numSensors = so->nr_locations; + { + int sensorCount = 0; + + for (int i = 0; i < so->nr_locations; i++) + { + if (fs->lengths[i][0][0] != -1 && fs->lengths[i][0][1] != -1) //lh 0 + { + to->sensor[sensorCount].normal.x = so->sensor_normals[i * 3 + 0]; + to->sensor[sensorCount].normal.y = so->sensor_normals[i * 3 + 1]; + to->sensor[sensorCount].normal.z = so->sensor_normals[i * 3 + 2]; + to->sensor[sensorCount].point.x = so->sensor_locations[i * 3 + 0]; + to->sensor[sensorCount].point.y = so->sensor_locations[i * 3 + 1]; + to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2]; + to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + sensorCount++; + } + } + + to->numSensors = sensorCount; + + SolveForLighthouse(to, 0); + } + { + int sensorCount = 0; + int lh = 1; + + for (int i = 0; i < so->nr_locations; i++) + { + if (fs->lengths[i][lh][0] != -1 && fs->lengths[i][lh][1] != -1) + { + to->sensor[sensorCount].normal.x = so->sensor_normals[i * 3 + 0]; + to->sensor[sensorCount].normal.y = so->sensor_normals[i * 3 + 1]; + to->sensor[sensorCount].normal.z = so->sensor_normals[i * 3 + 2]; + to->sensor[sensorCount].point.x = so->sensor_locations[i * 3 + 0]; + to->sensor[sensorCount].point.y = so->sensor_locations[i * 3 + 1]; + to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2]; + to->sensor[sensorCount].theta = fs->angles[i][lh][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = fs->angles[i][lh][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + sensorCount++; + } + } + + to->numSensors = sensorCount; + + SolveForLighthouse(to, 0); + } + //printf( "Full scene data.\n" ); + break; + } + case POSERDATA_DISASSOCIATE: + { + free( dd ); + so->PoserData = 0; + //printf( "Need to disassociate.\n" ); + break; + } + } + return 0; +} + + +REGISTER_LINKTIME( PoserTurveyTori ); + diff --git a/winbuild/libsurvive/libsurvive.vcxproj b/winbuild/libsurvive/libsurvive.vcxproj index 643cff5..05fec8c 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj +++ b/winbuild/libsurvive/libsurvive.vcxproj @@ -153,6 +153,7 @@ + diff --git a/winbuild/libsurvive/libsurvive.vcxproj.filters b/winbuild/libsurvive/libsurvive.vcxproj.filters index 0bb9d1b..8bb09b2 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj.filters +++ b/winbuild/libsurvive/libsurvive.vcxproj.filters @@ -84,6 +84,9 @@ Source Files + + Source Files + -- cgit v1.2.3 From 17db4d9c369ee8633b05e1d19b6fbb3d61007598 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Sun, 26 Mar 2017 09:34:08 -0700 Subject: Adding octavioradii OctavioRadii converges to the right radius!!!! --- src/poser_octavioradii.c | 542 +++++++++++++++++++++++++ src/poser_turveytori.c | 2 +- winbuild/libsurvive/libsurvive.vcxproj | 1 + winbuild/libsurvive/libsurvive.vcxproj.filters | 3 + 4 files changed, 547 insertions(+), 1 deletion(-) create mode 100644 src/poser_octavioradii.c diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c new file mode 100644 index 0000000..84d69fb --- /dev/null +++ b/src/poser_octavioradii.c @@ -0,0 +1,542 @@ +#include +#include +#include + +typedef struct +{ + int something; + //Stuff +} OctavioRadiiData; + +#include +#include +#include "linmath.h" +#include +#include +#include + +#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)) + +static FLT calculateFitnessOld(SensorAngles *angles, FLT *radii, PointPair *pairs, size_t numPairs) +{ + FLT fitness = 0; + for (size_t i = 0; i < numPairs; i++) + { + FLT estimatedDistanceBetweenPoints = + SQUARED(radii[pairs[i].index1]) + + SQUARED(radii[pairs[i].index2]) + - 2 * radii[pairs[i].index1] * radii[pairs[i].index2] + * FLT_SIN(angles[pairs[i].index1].HorizAngle) * FLT_SIN(angles[pairs[i].index2].HorizAngle) + * FLT_COS(angles[pairs[i].index1].VertAngle - angles[pairs[i].index2].VertAngle) + + FLT_COS(angles[pairs[i].index1].VertAngle) * FLT_COS(angles[pairs[i].index2].VertAngle); + + fitness += SQUARED(estimatedDistanceBetweenPoints - pairs[i].KnownDistance); + } + + return FLT_SQRT(fitness); +} + +// angles is an array of angles between a sensor pair +// pairs is an array of point pairs +// radii is the guess at the radii of those angles +static FLT calculateFitnessOld2(SensorAngles *angles, FLT *radii, PointPair *pairs, size_t numPairs) +{ + FLT fitness = 0; + for (size_t i = 0; i < numPairs; i++) + { + // These are the vectors that represent the direction for the two points. + // TODO: optimize by precomputing the tangent. + FLT v1[3], v2[3], diff[3]; + + v1[0] = 1; + v2[0] = 1; + v1[1] = tan(angles[pairs[i].index1].HorizAngle); // can be precomputed + v2[1] = tan(angles[pairs[i].index2].HorizAngle); // can be precomputed + v1[2] = tan(angles[pairs[i].index1].VertAngle); // can be precomputed + v2[2] = tan(angles[pairs[i].index2].VertAngle); // can be precomputed + + // Now, normalize the vectors + normalize3d(v1, v1); // can be precomputed + normalize3d(v2, v2); // can be precomputed + + // Now, given the specified radii, find where the new points are + scale3d(v1, v1, radii[pairs[i].index1]); + scale3d(v2, v2, radii[pairs[i].index2]); + + // Cool, now find the vector between these two points + // TODO: optimize the following two funcs into one. + sub3d(diff, v1, v2); + + FLT distance = magnitude3d(diff); + + FLT t1 = magnitude3d(v1); + FLT t2 = magnitude3d(v2); + + + + FLT estimatedDistanceBetweenPoints = + + SQUARED(radii[pairs[i].index1]) + + SQUARED(radii[pairs[i].index2]) + - 2 * radii[pairs[i].index1] * radii[pairs[i].index2] + * FLT_SIN(angles[pairs[i].index1].HorizAngle) * FLT_SIN(angles[pairs[i].index2].HorizAngle) + * FLT_COS(angles[pairs[i].index1].VertAngle - angles[pairs[i].index2].VertAngle) + + FLT_COS(angles[pairs[i].index1].VertAngle) * FLT_COS(angles[pairs[i].index2].VertAngle); + + + //fitness += SQUARED(estimatedDistanceBetweenPoints - pairs[i].KnownDistance); + fitness += SQUARED(distance - pairs[i].KnownDistance); + } + + return FLT_SQRT(fitness); +} + +static FLT angleBetweenSensors(SensorAngles *a, SensorAngles *b) +{ + FLT angle = FLT_ACOS(FLT_COS(a->VertAngle - b->VertAngle)*FLT_COS(a->HorizAngle - b->HorizAngle)); + //FLT angle2 = FLT_ACOS(FLT_COS(b->phi - a->phi)*FLT_COS(b->theta - a->theta)); + + return angle; +} + +// angles is an array of angles between a sensor pair +// pairs is an array of point pairs +// radii is the guess at the radii of those angles +static FLT calculateFitness(SensorAngles *angles, FLT *radii, PointPair *pairs, size_t numPairs) +{ + FLT fitness = 0; + for (size_t i = 0; i < numPairs; i++) + { + + FLT angle = angleBetweenSensors(&angles[pairs[i].index1], &angles[pairs[i].index2]); + + // now we have a side-angle-side triangle, and we need to find the third side. + + // The Law of Cosines says: a^2 = b^2 + c^2 ? 2bc * cosA, + // where A is the angle opposite side a. + + // Transform this to: + // a = sqrt(b^2 + c^2 - 2bc * cosA) and we know the length of the missing side! + + FLT b2 = (SQUARED(radii[pairs[i].index1])); + FLT c2 = (SQUARED(radii[pairs[i].index2])); + FLT bc2 = (2 * radii[pairs[i].index1] * radii[pairs[i].index2]); + FLT cosA = (FLT_COS(angle)); + + FLT angleInDegrees = angle * 180 / LINMATHPI; + + FLT dist = sqrt( (SQUARED(radii[pairs[i].index1])) + + (SQUARED(radii[pairs[i].index2])) - + ( (2 * radii[pairs[i].index1] * radii[pairs[i].index2]) * + (FLT_COS(angle)))); + + + FLT fitnessAdder = SQUARED(dist - pairs[i].KnownDistance); + + if (isnan(fitnessAdder)) + { + int a = 0; + } + + //printf(" %2d %f\n", i, fitnessAdder); + + //fitness += SQUARED(estimatedDistanceBetweenPoints - pairs[i].KnownDistance); + fitness += SQUARED(dist - pairs[i].KnownDistance); + } + + //fitness = 1 / fitness; + return FLT_SQRT(fitness); +} + +#define MAX_RADII 32 + +// note gradientOut will be of the same degree as numRadii +static void getGradient(FLT *gradientOut, SensorAngles *angles, FLT *radii, size_t numRadii, PointPair *pairs, size_t numPairs, const FLT precision) +{ + FLT baseline = calculateFitness(angles, radii, pairs, numPairs); + + for (size_t i = 0; i < numRadii; i++) + { + FLT tmpPlus[MAX_RADII]; + memcpy(tmpPlus, radii, sizeof(*radii) * numRadii); + tmpPlus[i] += precision; + gradientOut[i] = -(calculateFitness(angles, tmpPlus, pairs, numPairs) - baseline); + } + + return; +} + +static void normalizeAndMultiplyVector(FLT *vectorToNormalize, size_t count, FLT desiredMagnitude) +{ + FLT distanceIn = 0; + + for (size_t i = 0; i < count; i++) + { + distanceIn += SQUARED(vectorToNormalize[i]); + } + distanceIn = FLT_SQRT(distanceIn); + + + FLT scale = desiredMagnitude / distanceIn; + + for (size_t i = 0; i < count; i++) + { + vectorToNormalize[i] *= scale; + } + + return; +} + + +static RefineEstimateUsingGradientDescentRadii(FLT *estimateOut, SensorAngles *angles, FLT *initialEstimate, size_t numRadii, PointPair *pairs, size_t numPairs, FILE *logFile) +{ + int i = 0; + FLT lastMatchFitness = calculateFitness(angles, initialEstimate, pairs, numPairs); + if (estimateOut != initialEstimate) + { + memcpy(estimateOut, initialEstimate, sizeof(*estimateOut) * numRadii); + } + + + // The values below are somewhat magic, and definitely tunable + // The initial vlue of g will represent the biggest step that the gradient descent can take at first. + // bigger values may be faster, especially when the initial guess is wildly off. + // The downside to a bigger starting guess is that if we've picked a good guess at the local minima + // if there are other local minima, we may accidentally jump to such a local minima and get stuck there. + // That's fairly unlikely with the lighthouse problem, from expereince. + // The other downside is that if it's too big, we may have to spend a few iterations before it gets down + // to a size that doesn't jump us out of our minima. + // The terminal value of g represents how close we want to get to the local minima before we're "done" + // The change in value of g for each iteration is intentionally very close to 1. + // in fact, it probably could probably be 1 without any issue. The main place where g is decremented + // is in the block below when we've made a jump that results in a worse fitness than we're starting at. + // In those cases, we don't take the jump, and instead lower the value of g and try again. + for (FLT g = 0.4; g > 0.00001; g *= 0.9999) + { + i++; + + + + FLT point1[MAX_RADII]; + memcpy(point1, estimateOut, sizeof(*point1) * numRadii); + + // let's get 3 iterations of gradient descent here. + FLT gradient1[MAX_RADII]; + getGradient(gradient1, angles, point1, numRadii, pairs, numPairs, g / 1000 /*somewhat arbitrary*/); + normalizeAndMultiplyVector(gradient1, numRadii, g); + + FLT point2[MAX_RADII]; + for (size_t i = 0; i < numRadii; i++) + { + point2[i] = point1[i] + gradient1[i]; + } + FLT gradient2[MAX_RADII]; + getGradient(gradient2, angles, point2, numRadii, pairs, numPairs, g / 1000 /*somewhat arbitrary*/); + normalizeAndMultiplyVector(gradient2, numRadii, g); + + FLT point3[MAX_RADII]; + for (size_t i = 0; i < numRadii; i++) + { + point3[i] = point2[i] + gradient2[i]; + } + + // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley? + // Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic + // gradient descent is kinda horrible here. Instead, think about the shape that a zig-zagging + // converging gradient descent makes. Instead of using the gradient as the best indicator of + // the direction we should follow, we're looking at one side of the zig-zag pattern, and specifically + // following *that* vector. As it turns out, this works *amazingly* well. + + FLT specialGradient[MAX_RADII]; + for (size_t i = 0; i < numRadii; i++) + { + specialGradient[i] = point3[i] - point1[i]; + } + + // The second parameter to this function is very much a tunable parameter. Different values will result + // in a different number of iterations before we get to the minimum. Numbers between 3-10 seem to work well + // It's not clear what would be optimum here. + normalizeAndMultiplyVector(specialGradient, numRadii, g / 4); + + + FLT point4[MAX_RADII]; + for (size_t i = 0; i < numRadii; i++) + { + point4[i] = point3[i] + specialGradient[i]; + } + + + FLT newMatchFitness = calculateFitness(angles, point4, pairs, numPairs); + + + if (newMatchFitness < lastMatchFitness) + { + //if (logFile) + //{ + // writePoint(logFile, lastPoint.x, lastPoint.y, lastPoint.z, 0xFFFFFF); + //} + + lastMatchFitness = newMatchFitness; + memcpy(estimateOut, point4, sizeof(*estimateOut) * numRadii); + +#ifdef RADII_DEBUG + printf("+ %d %0.9f (%0.9f) \n", i, newMatchFitness, g); +#endif + g = g * 1.05; + } + else + { +//#ifdef RADII_DEBUG + // printf("-"); + printf("- %d %0.9f (%0.9f) [%0.9f] \n", i, newMatchFitness, g, estimateOut[0]); +//#endif + // if it wasn't a match, back off on the distance we jump + g *= 0.7; + + } + +#ifdef RADII_DEBUG + FLT avg = 0; + FLT diffFromAvg[MAX_RADII]; + + for (size_t m = 0; m < numRadii; m++) + { + avg += estimateOut[m]; + } + avg = avg / numRadii; + + for (size_t m = 0; m < numRadii; m++) + { + diffFromAvg[m] = estimateOut[m] - avg;; + } + printf("[avg:%f] ", avg); + + for (size_t x = 0; x < numRadii; x++) + { + printf("%f, ", diffFromAvg[x]); + //printf("%f, ", estimateOut[x]); + } + printf("\n"); + + +#endif + + + } + printf("\ni=%d\n", i); +} + +void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObject *obj) +{ + FLT estimate[MAX_RADII]; + + for (size_t i = 0; i < MAX_RADII; i++) + { + estimate[i] = 2.2; + } + + SensorAngles angles[MAX_RADII]; + PointPair pairs[MAX_POINT_PAIRS]; + + size_t pairCount = 0; + + //obj->numSensors = 5; // TODO: HACK!!!! + + for (size_t i = 0; i < obj->numSensors; i++) + { + angles[i].HorizAngle = obj->sensor[i].theta; + angles[i].VertAngle = obj->sensor[i].phi; + } + + for (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++; + } + } + + + RefineEstimateUsingGradientDescentRadii(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) + + return; +} + +int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) +{ + PoserType pt = pd->pt; + SurviveContext * ctx = so->ctx; + OctavioRadiiData * dd = so->PoserData; + + if( !dd ) so->PoserData = dd = malloc( sizeof(OctavioRadiiData) ); + + 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: + { + TrackedObject *to; + + PoserDataFullScene * fs = (PoserDataFullScene*)pd; + + to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor))); + + //FLT lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; + //FLT angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; //2 Axes (Angles in LH space) + //FLT synctimes[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES]; + + //to->numSensors = so->nr_locations; + { + int sensorCount = 0; + + for (int i = 0; i < so->nr_locations; i++) + { + if (fs->lengths[i][0][0] != -1 && fs->lengths[i][0][1] != -1) //lh 0 + { + to->sensor[sensorCount].normal.x = so->sensor_normals[i * 3 + 0]; + to->sensor[sensorCount].normal.y = so->sensor_normals[i * 3 + 1]; + to->sensor[sensorCount].normal.z = so->sensor_normals[i * 3 + 2]; + to->sensor[sensorCount].point.x = so->sensor_locations[i * 3 + 0]; + to->sensor[sensorCount].point.y = so->sensor_locations[i * 3 + 1]; + to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2]; + to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + sensorCount++; + } + } + + to->numSensors = sensorCount; + + Point position; + FLT orientation[4]; + + SolveForLighthouseRadii(&position, &orientation, to); + } + { + int sensorCount = 0; + int lh = 1; + + for (int i = 0; i < so->nr_locations; i++) + { + if (fs->lengths[i][lh][0] != -1 && fs->lengths[i][lh][1] != -1) + { + to->sensor[sensorCount].normal.x = so->sensor_normals[i * 3 + 0]; + to->sensor[sensorCount].normal.y = so->sensor_normals[i * 3 + 1]; + to->sensor[sensorCount].normal.z = so->sensor_normals[i * 3 + 2]; + to->sensor[sensorCount].point.x = so->sensor_locations[i * 3 + 0]; + to->sensor[sensorCount].point.y = so->sensor_locations[i * 3 + 1]; + to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2]; + to->sensor[sensorCount].theta = fs->angles[i][lh][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = fs->angles[i][lh][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + sensorCount++; + } + } + + to->numSensors = sensorCount; + + Point position; + FLT orientation[4]; + + SolveForLighthouseRadii(&position, &orientation, to); + } + //printf( "Full scene data.\n" ); + break; + } + case POSERDATA_DISASSOCIATE: + { + free( dd ); + so->PoserData = 0; + //printf( "Need to disassociate.\n" ); + break; + } + } + return 0; +} + + +REGISTER_LINKTIME( PoserOctavioRadii ); + diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index e9e5b7a..4e602f3 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -299,7 +299,7 @@ void estimateToroidalAndPoloidalAngleOfPoint( FLT angleBetweenSensors(TrackedSensor *a, TrackedSensor *b) { FLT angle = FLT_ACOS(FLT_COS(a->phi - b->phi)*FLT_COS(a->theta - b->theta)); - FLT angle2 = FLT_ACOS(FLT_COS(b->phi - a->phi)*FLT_COS(b->theta - a->theta)); + //FLT angle2 = FLT_ACOS(FLT_COS(b->phi - a->phi)*FLT_COS(b->theta - a->theta)); return angle; } diff --git a/winbuild/libsurvive/libsurvive.vcxproj b/winbuild/libsurvive/libsurvive.vcxproj index 05fec8c..c794382 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj +++ b/winbuild/libsurvive/libsurvive.vcxproj @@ -153,6 +153,7 @@ + diff --git a/winbuild/libsurvive/libsurvive.vcxproj.filters b/winbuild/libsurvive/libsurvive.vcxproj.filters index 8bb09b2..e7d44e2 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj.filters +++ b/winbuild/libsurvive/libsurvive.vcxproj.filters @@ -87,6 +87,9 @@ Source Files + + Source Files + -- cgit v1.2.3 From 51b8cf7083d712eac730fe90f383abfabc7f2676 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Sun, 26 Mar 2017 11:49:51 -0700 Subject: Tweaking --- src/poser_octavioradii.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c index 84d69fb..18d4026 100644 --- a/src/poser_octavioradii.c +++ b/src/poser_octavioradii.c @@ -350,7 +350,7 @@ static RefineEstimateUsingGradientDescentRadii(FLT *estimateOut, SensorAngles *a { //#ifdef RADII_DEBUG // printf("-"); - printf("- %d %0.9f (%0.9f) [%0.9f] \n", i, newMatchFitness, g, estimateOut[0]); + //printf("- %d %0.9f (%0.9f) [%0.9f] \n", i, newMatchFitness, g, estimateOut[0]); //#endif // if it wasn't a match, back off on the distance we jump g *= 0.7; @@ -394,7 +394,7 @@ void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObj for (size_t i = 0; i < MAX_RADII; i++) { - estimate[i] = 2.2; + estimate[i] = 2.38; } SensorAngles angles[MAX_RADII]; -- cgit v1.2.3 From 5ef6668c3d2bc0a4e7c364119a99fcd6c6e43017 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Sun, 26 Mar 2017 19:08:44 -0700 Subject: Adding svd --- redist/svd.h | 450 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 450 insertions(+) create mode 100644 redist/svd.h diff --git a/redist/svd.h b/redist/svd.h new file mode 100644 index 0000000..41708da --- /dev/null +++ b/redist/svd.h @@ -0,0 +1,450 @@ +/************************************************************************** +** +** svd3 +** +** Quick singular value decomposition as described by: +** A. McAdams, A. Selle, R. Tamstorf, J. Teran and E. Sifakis, +** "Computing the Singular Value Decomposition of 3x3 matrices +** with minimal branching and elementary floating point operations", +** University of Wisconsin - Madison technical report TR1690, May 2011 +** +** OPTIMIZED CPU VERSION +** Implementation by: Eric Jang +** +** 13 Apr 2014 +** +** This file originally retrieved from: +** https://github.com/ericjang/svd3/blob/master/svd3.h 3/26/2017 +** +** Original licesnse is MIT per: +** https://github.com/ericjang/svd3/blob/master/LICENSE.md +** +** Ported from C++ to C by Mike Turvey. All modifications also released +** under an MIT license. +**************************************************************************/ + + +#ifndef SVD3_H +#define SVD3_H + +#define _gamma 5.828427124 // FOUR_GAMMA_SQUARED = sqrt(8)+3; +#define _cstar 0.923879532 // cos(pi/8) +#define _sstar 0.3826834323 // sin(p/8) +#define EPSILON 1e-6 + +#include + +/* This is a novel and fast routine for the reciprocal square root of an +IEEE float (single precision). +http://www.lomont.org/Math/Papers/2003/InvSqrt.pdf +http://playstation2-linux.com/download/p2lsd/fastrsqrt.pdf +http://www.beyond3d.com/content/articles/8/ +*/ +inline float rsqrt(float x) { + // int ihalf = *(int *)&x - 0x00800000; // Alternative to next line, + // float xhalf = *(float *)&ihalf; // for sufficiently large nos. + float xhalf = 0.5f*x; + int i = *(int *)&x; // View x as an int. + // i = 0x5f3759df - (i >> 1); // Initial guess (traditional). + i = 0x5f375a82 - (i >> 1); // Initial guess (slightly better). + x = *(float *)&i; // View i as float. + x = x*(1.5f - xhalf*x*x); // Newton step. + // x = x*(1.5008908 - xhalf*x*x); // Newton step for a balanced error. + return x; +} + +/* This is rsqrt with an additional step of the Newton iteration, for +increased accuracy. The constant 0x5f37599e makes the relative error +range from 0 to -0.00000463. +You can't balance the error by adjusting the constant. */ +inline float rsqrt1(float x) { + float xhalf = 0.5f*x; + int i = *(int *)&x; // View x as an int. + i = 0x5f37599e - (i >> 1); // Initial guess. + x = *(float *)&i; // View i as float. + x = x*(1.5f - xhalf*x*x); // Newton step. + x = x*(1.5f - xhalf*x*x); // Newton step again. + return x; +} + +inline float accurateSqrt(float x) +{ + return x * rsqrt1(x); +} + +inline void condSwap(bool c, float *X, float *Y) +{ + // used in step 2 + float Z = *X; + *X = c ? *Y : *X; + *Y = c ? Z : *Y; +} + +inline void condNegSwap(bool c, float *X, float *Y) +{ + // used in step 2 and 3 + float Z = -*X; + *X = c ? *Y : *X; + *Y = c ? Z : *Y; +} + +// matrix multiplication M = A * B +inline void multAB(float a11, float a12, float a13, + float a21, float a22, float a23, + float a31, float a32, float a33, + // + float b11, float b12, float b13, + float b21, float b22, float b23, + float b31, float b32, float b33, + // + float *m11, float *m12, float *m13, + float *m21, float *m22, float *m23, + float *m31, float *m32, float *m33) +{ + + *m11 = a11*b11 + a12*b21 + a13*b31; *m12 = a11*b12 + a12*b22 + a13*b32; *m13 = a11*b13 + a12*b23 + a13*b33; + *m21 = a21*b11 + a22*b21 + a23*b31; *m22 = a21*b12 + a22*b22 + a23*b32; *m23 = a21*b13 + a22*b23 + a23*b33; + *m31 = a31*b11 + a32*b21 + a33*b31; *m32 = a31*b12 + a32*b22 + a33*b32; *m33 = a31*b13 + a32*b23 + a33*b33; +} + +// matrix multiplication M = Transpose[A] * B +inline void multAtB(float a11, float a12, float a13, + float a21, float a22, float a23, + float a31, float a32, float a33, + // + float b11, float b12, float b13, + float b21, float b22, float b23, + float b31, float b32, float b33, + // + float *m11, float *m12, float *m13, + float *m21, float *m22, float *m23, + float *m31, float *m32, float *m33) +{ + *m11 = a11*b11 + a21*b21 + a31*b31; *m12 = a11*b12 + a21*b22 + a31*b32; *m13 = a11*b13 + a21*b23 + a31*b33; + *m21 = a12*b11 + a22*b21 + a32*b31; *m22 = a12*b12 + a22*b22 + a32*b32; *m23 = a12*b13 + a22*b23 + a32*b33; + *m31 = a13*b11 + a23*b21 + a33*b31; *m32 = a13*b12 + a23*b22 + a33*b32; *m33 = a13*b13 + a23*b23 + a33*b33; +} + +inline void quatToMat3(const float * qV, + float *m11, float *m12, float *m13, + float *m21, float *m22, float *m23, + float *m31, float *m32, float *m33 +) +{ + float w = qV[3]; + float x = qV[0]; + float y = qV[1]; + float z = qV[2]; + + float qxx = x*x; + float qyy = y*y; + float qzz = z*z; + float qxz = x*z; + float qxy = x*y; + float qyz = y*z; + float qwx = w*x; + float qwy = w*y; + float qwz = w*z; + + *m11 = 1 - 2 * (qyy + qzz); *m12 = 2 * (qxy - qwz); *m13 = 2 * (qxz + qwy); + *m21 = 2 * (qxy + qwz); *m22 = 1 - 2 * (qxx + qzz); *m23 = 2 * (qyz - qwx); + *m31 = 2 * (qxz - qwy); *m32 = 2 * (qyz + qwx); *m33 = 1 - 2 * (qxx + qyy); +} + +inline void approximateGivensQuaternion(float a11, float a12, float a22, float *ch, float *sh) +{ + /* + * Given givens angle computed by approximateGivensAngles, + * compute the corresponding rotation quaternion. + */ + *ch = 2 * (a11 - a22); + *sh = a12; + bool b = _gamma* (*sh)*(*sh) < (*ch)*(*ch); + // fast rsqrt function suffices + // rsqrt2 (https://code.google.com/p/lppython/source/browse/algorithm/HDcode/newCode/rsqrt.c?r=26) + // is even faster but results in too much error + float w = rsqrt((*ch)*(*ch) + (*sh)*(*sh)); + *ch = b ? w*(*ch) : (float)_cstar; + *sh = b ? w*(*sh) : (float)_sstar; +} + +inline void jacobiConjugation(const int x, const int y, const int z, + float *s11, + float *s21, float *s22, + float *s31, float *s32, float *s33, + float * qV) +{ + float ch, sh; + approximateGivensQuaternion(*s11, *s21, *s22, &ch, &sh); + + float scale = ch*ch + sh*sh; + float a = (ch*ch - sh*sh) / scale; + float b = (2 * sh*ch) / scale; + + // make temp copy of S + float _s11 = *s11; + float _s21 = *s21; float _s22 = *s22; + float _s31 = *s31; float _s32 = *s32; float _s33 = *s33; + + // perform conjugation S = Q'*S*Q + // Q already implicitly solved from a, b + *s11 = a*(a*_s11 + b*_s21) + b*(a*_s21 + b*_s22); + *s21 = a*(-b*_s11 + a*_s21) + b*(-b*_s21 + a*_s22); *s22 = -b*(-b*_s11 + a*_s21) + a*(-b*_s21 + a*_s22); + *s31 = a*_s31 + b*_s32; *s32 = -b*_s31 + a*_s32; *s33 = _s33; + + // update cumulative rotation qV + float tmp[3]; + tmp[0] = qV[0] * sh; + tmp[1] = qV[1] * sh; + tmp[2] = qV[2] * sh; + sh *= qV[3]; + + qV[0] *= ch; + qV[1] *= ch; + qV[2] *= ch; + qV[3] *= ch; + + // (x,y,z) corresponds to ((0,1,2),(1,2,0),(2,0,1)) + // for (p,q) = ((0,1),(1,2),(0,2)) + qV[z] += sh; + qV[3] -= tmp[z]; // w + qV[x] += tmp[y]; + qV[y] -= tmp[x]; + + // re-arrange matrix for next iteration + _s11 = *s22; + _s21 = *s32; _s22 = *s33; + _s31 = *s21; _s32 = *s31; _s33 = *s11; + *s11 = _s11; + *s21 = _s21; *s22 = _s22; + *s31 = _s31; *s32 = _s32; *s33 = _s33; + +} + +inline float dist2(float x, float y, float z) +{ + return x*x + y*y + z*z; +} + +// finds transformation that diagonalizes a symmetric matrix +inline void jacobiEigenanlysis( // symmetric matrix + float *s11, + float *s21, float *s22, + float *s31, float *s32, float *s33, + // quaternion representation of V + float * qV) +{ + qV[3] = 1; qV[0] = 0; qV[1] = 0; qV[2] = 0; // follow same indexing convention as GLM + for (int i = 0; i<4; i++) + { + // we wish to eliminate the maximum off-diagonal element + // on every iteration, but cycling over all 3 possible rotations + // in fixed order (p,q) = (1,2) , (2,3), (1,3) still retains + // asymptotic convergence + jacobiConjugation(0, 1, 2, s11, s21, s22, s31, s32, s33, qV); // p,q = 0,1 + jacobiConjugation(1, 2, 0, s11, s21, s22, s31, s32, s33, qV); // p,q = 1,2 + jacobiConjugation(2, 0, 1, s11, s21, s22, s31, s32, s33, qV); // p,q = 0,2 + } +} + + +inline void sortSingularValues(// matrix that we want to decompose + float *b11, float *b12, float *b13, + float *b21, float *b22, float *b23, + float *b31, float *b32, float *b33, + // sort V simultaneously + float *v11, float *v12, float *v13, + float *v21, float *v22, float *v23, + float *v31, float *v32, float *v33) +{ + float rho1 = dist2(*b11, *b21, *b31); + float rho2 = dist2(*b12, *b22, *b32); + float rho3 = dist2(*b13, *b23, *b33); + bool c; + c = rho1 < rho2; + condNegSwap(c, b11, b12); condNegSwap(c, v11, v12); + condNegSwap(c, b21, b22); condNegSwap(c, v21, v22); + condNegSwap(c, b31, b32); condNegSwap(c, v31, v32); + condSwap(c, &rho1, &rho2); + c = rho1 < rho3; + condNegSwap(c, b11, b13); condNegSwap(c, v11, v13); + condNegSwap(c, b21, b23); condNegSwap(c, v21, v23); + condNegSwap(c, b31, b33); condNegSwap(c, v31, v33); + condSwap(c, &rho1, &rho3); + c = rho2 < rho3; + condNegSwap(c, b12, b13); condNegSwap(c, v12, v13); + condNegSwap(c, b22, b23); condNegSwap(c, v22, v23); + condNegSwap(c, b32, b33); condNegSwap(c, v32, v33); +} + + +void QRGivensQuaternion(float a1, float a2, float *ch, float *sh) +{ + // a1 = pivot point on diagonal + // a2 = lower triangular entry we want to annihilate + float epsilon = (float)EPSILON; + float rho = accurateSqrt(a1*a1 + a2*a2); + + *sh = rho > epsilon ? a2 : 0; + *ch = fabsf(a1) + fmaxf(rho, epsilon); + bool b = a1 < 0; + condSwap(b, sh, ch); + float w = rsqrt((*ch)*(*ch) + (*sh)*(*sh)); + *ch *= w; + *sh *= w; +} + + +inline void QRDecomposition(// matrix that we want to decompose + float b11, float b12, float b13, + float b21, float b22, float b23, + float b31, float b32, float b33, + // output Q + float *q11, float *q12, float *q13, + float *q21, float *q22, float *q23, + float *q31, float *q32, float *q33, + // output R + float *r11, float *r12, float *r13, + float *r21, float *r22, float *r23, + float *r31, float *r32, float *r33) +{ + float ch1, sh1, ch2, sh2, ch3, sh3; + float a, b; + + // first givens rotation (ch,0,0,sh) + QRGivensQuaternion(b11, b21, &ch1, &sh1); + a = 1 - 2 * sh1*sh1; + b = 2 * ch1*sh1; + // apply B = Q' * B + *r11 = a*b11 + b*b21; *r12 = a*b12 + b*b22; *r13 = a*b13 + b*b23; + *r21 = -b*b11 + a*b21; *r22 = -b*b12 + a*b22; *r23 = -b*b13 + a*b23; + *r31 = b31; *r32 = b32; *r33 = b33; + + // second givens rotation (ch,0,-sh,0) + QRGivensQuaternion(*r11, *r31, &ch2, &sh2); + a = 1 - 2 * sh2*sh2; + b = 2 * ch2*sh2; + // apply B = Q' * B; + b11 = a*(*r11) + b*(*r31); b12 = a*(*r12) + b*(*r32); b13 = a*(*r13) + b*(*r33); + b21 = *r21; b22 = *r22; b23 = *r23; + b31 = -b*(*r11) + a*(*r31); b32 = -b*(*r12) + a*(*r32); b33 = -b*(*r13) + a*(*r33); + + // third givens rotation (ch,sh,0,0) + QRGivensQuaternion(b22, b32, &ch3, &sh3); + a = 1 - 2 * sh3*sh3; + b = 2 * ch3*sh3; + // R is now set to desired value + *r11 = b11; *r12 = b12; *r13 = b13; + *r21 = a*b21 + b*b31; *r22 = a*b22 + b*b32; *r23 = a*b23 + b*b33; + *r31 = -b*b21 + a*b31; *r32 = -b*b22 + a*b32; *r33 = -b*b23 + a*b33; + + // construct the cumulative rotation Q=Q1 * Q2 * Q3 + // the number of floating point operations for three quaternion multiplications + // is more or less comparable to the explicit form of the joined matrix. + // certainly more memory-efficient! + float sh12 = sh1*sh1; + float sh22 = sh2*sh2; + float sh32 = sh3*sh3; + + *q11 = (-1 + 2 * sh12)*(-1 + 2 * sh22); + *q12 = 4 * ch2*ch3*(-1 + 2 * sh12)*sh2*sh3 + 2 * ch1*sh1*(-1 + 2 * sh32); + *q13 = 4 * ch1*ch3*sh1*sh3 - 2 * ch2*(-1 + 2 * sh12)*sh2*(-1 + 2 * sh32); + + *q21 = 2 * ch1*sh1*(1 - 2 * sh22); + *q22 = -8 * ch1*ch2*ch3*sh1*sh2*sh3 + (-1 + 2 * sh12)*(-1 + 2 * sh32); + *q23 = -2 * ch3*sh3 + 4 * sh1*(ch3*sh1*sh3 + ch1*ch2*sh2*(-1 + 2 * sh32)); + + *q31 = 2 * ch2*sh2; + *q32 = 2 * ch3*(1 - 2 * sh22)*sh3; + *q33 = (-1 + 2 * sh22)*(-1 + 2 * sh32); +} + +void svd(// input A + float a11, float a12, float a13, + float a21, float a22, float a23, + float a31, float a32, float a33, + // output U + float *u11, float *u12, float *u13, + float *u21, float *u22, float *u23, + float *u31, float *u32, float *u33, + // output S + float *s11, float *s12, float *s13, + float *s21, float *s22, float *s23, + float *s31, float *s32, float *s33, + // output V + float *v11, float *v12, float *v13, + float *v21, float *v22, float *v23, + float *v31, float *v32, float *v33) +{ + // normal equations matrix + float ATA11, ATA12, ATA13; + float ATA21, ATA22, ATA23; + float ATA31, ATA32, ATA33; + + multAtB(a11, a12, a13, a21, a22, a23, a31, a32, a33, + a11, a12, a13, a21, a22, a23, a31, a32, a33, + &ATA11, &ATA12, &ATA13, &ATA21, &ATA22, &ATA23, &ATA31, &ATA32, &ATA33); + + // symmetric eigenalysis + float qV[4]; + jacobiEigenanlysis(&ATA11, &ATA21, &ATA22, &ATA31, &ATA32, &ATA33, qV); + quatToMat3(qV, v11, v12, v13, v21, v22, v23, v31, v32, v33); + + float b11, b12, b13; + float b21, b22, b23; + float b31, b32, b33; + multAB(a11, a12, a13, a21, a22, a23, a31, a32, a33, + *v11, *v12, *v13, *v21, *v22, *v23, *v31, *v32, *v33, + &b11, &b12, &b13, &b21, &b22, &b23, &b31, &b32, &b33); + + // sort singular values and find V + sortSingularValues(&b11, &b12, &b13, &b21, &b22, &b23, &b31, &b32, &b33, + v11, v12, v13, v21, v22, v23, v31, v32, v33); + + // QR decomposition + QRDecomposition(b11, b12, b13, b21, b22, b23, b31, b32, b33, + u11, u12, u13, u21, u22, u23, u31, u32, u33, + s11, s12, s13, s21, s22, s23, s31, s32, s33 + ); +} + +/// polar decomposition can be reconstructed trivially from SVD result +// A = UP +void pd(float a11, float a12, float a13, + float a21, float a22, float a23, + float a31, float a32, float a33, + // output U + float *u11, float *u12, float *u13, + float *u21, float *u22, float *u23, + float *u31, float *u32, float *u33, + // output P + float *p11, float *p12, float *p13, + float *p21, float *p22, float *p23, + float *p31, float *p32, float *p33) +{ + float w11, w12, w13, w21, w22, w23, w31, w32, w33; + float s11, s12, s13, s21, s22, s23, s31, s32, s33; + float v11, v12, v13, v21, v22, v23, v31, v32, v33; + + svd(a11, a12, a13, a21, a22, a23, a31, a32, a33, + &w11, &w12, &w13, &w21, &w22, &w23, &w31, &w32, &w33, + &s11, &s12, &s13, &s21, &s22, &s23, &s31, &s32, &s33, + &v11, &v12, &v13, &v21, &v22, &v23, &v31, &v32, &v33); + + // P = VSV' + float t11, t12, t13, t21, t22, t23, t31, t32, t33; + multAB(v11, v12, v13, v21, v22, v23, v31, v32, v33, + s11, s12, s13, s21, s22, s23, s31, s32, s33, + &t11, &t12, &t13, &t21, &t22, &t23, &t31, &t32, &t33); + + multAB(t11, t12, t13, t21, t22, t23, t31, t32, t33, + v11, v21, v31, v12, v22, v32, v13, v23, v33, + p11, p12, p13, p21, p22, p23, p31, p32, p33); + + // U = WV' + multAB(w11, w12, w13, w21, w22, w23, w31, w32, w33, + v11, v21, v31, v12, v22, v32, v13, v23, v33, + u11, u12, u13, u21, u22, u23, u31, u32, u33); +} + +#endif \ No newline at end of file -- cgit v1.2.3 From ef039bb3ee8ce95f25e86baddbc4b3d5f21e6743 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 10:20:05 -0700 Subject: Fixing compiler warnings --- src/poser_octavioradii.c | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c index 18d4026..3893085 100644 --- a/src/poser_octavioradii.c +++ b/src/poser_octavioradii.c @@ -24,14 +24,11 @@ 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 @@ -410,9 +407,9 @@ void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObj angles[i].VertAngle = obj->sensor[i].phi; } - for (size_t i = 0; i < obj->numSensors - 1; i++) + for (unsigned char i = 0; i < obj->numSensors - 1; i++) { - for (size_t j = i + 1; j < obj->numSensors; j++) + for (unsigned char j = i + 1; j < obj->numSensors; j++) { pairs[pairCount].index1 = i; pairs[pairCount].index2 = j; @@ -426,7 +423,7 @@ void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObj // we should now have an estimate of the radii. - for (size_t i = 0; i < obj->numSensors; i++) + for (int i = 0; i < obj->numSensors; i++) { printf("radius[%d]: %f\n", i, estimate[i]); } @@ -494,7 +491,7 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) Point position; FLT orientation[4]; - SolveForLighthouseRadii(&position, &orientation, to); + SolveForLighthouseRadii(&position, orientation, to); } { int sensorCount = 0; @@ -521,7 +518,7 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) Point position; FLT orientation[4]; - SolveForLighthouseRadii(&position, &orientation, to); + SolveForLighthouseRadii(&position, orientation, to); } //printf( "Full scene data.\n" ); break; -- cgit v1.2.3 From f923e3c5ad03e7942eb46b67666807b738a1428a Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 10:59:26 -0700 Subject: completed fitness func for tori poser rotation --- src/poser_turveytori.c | 60 ++++++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 53 insertions(+), 7 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 4e602f3..37f79bb 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -535,23 +535,69 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, // 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) { + FLT fitness = 0; for (size_t i = 0; i < obj->numSensors; i++) { // first, get the normal of the plane for the horizonal sweep FLT theta = obj->sensor[i].theta; // make two vectors that lie on the plane - FLT t1[3] = { 1, tan(theta), 0 }; - FLT t2[3] = { 1, tan(theta), 1 }; + FLT t1H[3] = { 1, tan(theta), 0 }; + FLT t2H[3] = { 1, tan(theta), 1 }; - FLT tNorm[3]; + FLT tNormH[3]; // the normal is the cross of two vectors on the plane. - cross3d(tNorm, t1, t2); + cross3d(tNormH, t1H, t2H); - // 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 = + normalize3d(tNormH, tNormH); + + // Now do the same for the vertical sweep + + // first, get the normal of the plane for the horizonal sweep + FLT phi = obj->sensor[i].phi; + // make two vectors that lie on the plane + FLT t1V[3] = { 0, 1, tan(phi)}; + FLT t2V[3] = { 1, 1, tan(phi)}; + + FLT tNormV[3]; + + // the normal is the cross of two vectors on the plane. + cross3d(tNormV, t1V, t2V); + + normalize3d(tNormV, tNormV); + + + // First, where is the sensor in the object's reference frame? + FLT sensor_in_obj_reference_frame[3] = {obj->sensor->point.x, obj->sensor->point.y, obj->sensor->point.z}; + // Where is the point, in the reference frame of the lighthouse? + // This has two steps, first we translate from the object's location being the + // origin to the lighthouse being the origin. + // And second, we apply the quaternion to rotate into the proper reference frame for the lighthouse. + + FLT sensor_in_lh_reference_frame[3]; + sub3d(sensor_in_lh_reference_frame, sensor_in_obj_reference_frame, (FLT[3]){lhPoint.x, lhPoint.y, lhPoint.z}); + + quatrotatevector(sensor_in_lh_reference_frame, quaternion, sensor_in_lh_reference_frame); + + // now the we've got the location of the sensor in the lighthouses's reference frame, given lhPoint and quaternion inputs. + + // We need an arbitrary vector from the plane to the point. + // Since the plane goes through the origin, this is trivial. + // The sensor point itself is such a vector! + + // And go calculate the distances! + // TODO: don't need to ABS these because we square them below. + FLT dH = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormH)); + FLT dV = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormV)); + + + fitness += SQUARED(dH); + fitness += SQUARED(dV); } + + fitness = FLT_SQRT(fitness); + + return fitness; } static Point RefineRotationEstimate(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, FILE *logFile) -- cgit v1.2.3 From b9e9c89a46a91cbc69d7832d6f67e571723d11e6 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 12:48:40 -0700 Subject: Tori puzzle pieces in place, rotation not converging --- src/poser_turveytori.c | 197 +++++++++++++++++++++++++++++++++++++------------ 1 file changed, 149 insertions(+), 48 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 37f79bb..15961c8 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -412,7 +412,7 @@ Point getGradient(Point pointIn, PointsAndAngle *pna, size_t pnaCount, FLT preci return result; } -Point getNormalizedVector(Point vectorIn, FLT desiredMagnitude) +Point getNormalizedAndScaledVector(Point vectorIn, FLT desiredMagnitude) { FLT distanceIn = sqrt(SQUARED(vectorIn.x) + SQUARED(vectorIn.y) + SQUARED(vectorIn.z)); @@ -464,7 +464,7 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, 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 gradientN1 = getNormalizedAndScaledVector(gradient1, g); Point point2; point2.x = point1.x + gradientN1.x; @@ -472,7 +472,7 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, point2.z = point1.z + gradientN1.z; Point gradient2 = getGradient(point2, pna, pnaCount, g / 1000 /*somewhat arbitrary*/); - Point gradientN2 = getNormalizedVector(gradient2, g); + Point gradientN2 = getNormalizedAndScaledVector(gradient2, g); Point point3; point3.x = point2.x + gradientN2.x; @@ -491,7 +491,7 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, // 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); + specialGradient = getNormalizedAndScaledVector(specialGradient, g / 4); Point point4; @@ -531,6 +531,75 @@ 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 RotationEstimateFitnessOld(Point lhPoint, FLT *quaternion, TrackedObject *obj) +{ + FLT fitness = 0; + for (size_t i = 0; i < obj->numSensors; i++) + { + // first, get the normal of the plane for the horizonal sweep + FLT theta = obj->sensor[i].theta; + // make two vectors that lie on the plane + FLT t1H[3] = { 1, tan(theta-LINMATHPI/2), 0 }; + FLT t2H[3] = { 1, tan(theta-LINMATHPI/2), 1 }; + + FLT tNormH[3]; + + // the normal is the cross of two vectors on the plane. + cross3d(tNormH, t1H, t2H); + + normalize3d(tNormH, tNormH); + + // Now do the same for the vertical sweep + + // first, get the normal of the plane for the horizonal sweep + FLT phi = obj->sensor[i].phi; + // make two vectors that lie on the plane + FLT t1V[3] = { 0, 1, tan(phi-LINMATHPI/2)}; + FLT t2V[3] = { 1, 1, tan(phi-LINMATHPI/2)}; + + FLT tNormV[3]; + + // the normal is the cross of two vectors on the plane. + cross3d(tNormV, t1V, t2V); + + normalize3d(tNormV, tNormV); + + + // First, where is the sensor in the object's reference frame? + FLT sensor_in_obj_reference_frame[3] = {obj->sensor->point.x, obj->sensor->point.y, obj->sensor->point.z}; + // Where is the point, in the reference frame of the lighthouse? + // This has two steps, first we translate from the object's location being the + // origin to the lighthouse being the origin. + // And second, we apply the quaternion to rotate into the proper reference frame for the lighthouse. + + FLT sensor_in_lh_reference_frame[3]; + sub3d(sensor_in_lh_reference_frame, sensor_in_obj_reference_frame, (FLT[3]){lhPoint.x, lhPoint.y, lhPoint.z}); + + quatrotatevector(sensor_in_lh_reference_frame, quaternion, sensor_in_lh_reference_frame); + + // now the we've got the location of the sensor in the lighthouses's reference frame, given lhPoint and quaternion inputs. + + // We need an arbitrary vector from the plane to the point. + // Since the plane goes through the origin, this is trivial. + // The sensor point itself is such a vector! + + // And go calculate the distances! + // TODO: don't need to ABS these because we square them below. + FLT dH = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormH)); + FLT dV = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormV)); + + + fitness += SQUARED(dH); + fitness += SQUARED(dV); + } + + fitness = FLT_SQRT(fitness); + + return fitness; +} + // 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) @@ -541,8 +610,8 @@ FLT RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj) // first, get the normal of the plane for the horizonal sweep FLT theta = obj->sensor[i].theta; // make two vectors that lie on the plane - FLT t1H[3] = { 1, tan(theta), 0 }; - FLT t2H[3] = { 1, tan(theta), 1 }; + FLT t1H[3] = { 1, tan(theta-LINMATHPI/2), 0 }; + FLT t2H[3] = { 1, tan(theta-LINMATHPI/2), 1 }; FLT tNormH[3]; @@ -556,8 +625,8 @@ FLT RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj) // first, get the normal of the plane for the horizonal sweep FLT phi = obj->sensor[i].phi; // make two vectors that lie on the plane - FLT t1V[3] = { 0, 1, tan(phi)}; - FLT t2V[3] = { 1, 1, tan(phi)}; + FLT t1V[3] = { 0, 1, tan(phi-LINMATHPI/2)}; + FLT t2V[3] = { 1, 1, tan(phi-LINMATHPI/2)}; FLT tNormV[3]; @@ -600,11 +669,43 @@ FLT RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj) return fitness; } -static Point RefineRotationEstimate(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, FILE *logFile) +void getRotationGradient(FLT *gradientOut, Point lhPoint, FLT *quaternion, TrackedObject *obj, FLT precision) +{ + + FLT baseFitness = RotationEstimateFitness(lhPoint, quaternion, obj); + + FLT tmp0plus[4]; + quatadd(tmp0plus, quaternion, (FLT[4]){precision, 0, 0, 0}); + gradientOut[0] = RotationEstimateFitness(lhPoint, tmp0plus, obj) - baseFitness; + + FLT tmp1plus[4]; + quatadd(tmp1plus, quaternion, (FLT[4]){0, precision, 0, 0}); + gradientOut[1] = RotationEstimateFitness(lhPoint, tmp1plus, obj) - baseFitness; + + FLT tmp2plus[4]; + quatadd(tmp2plus, quaternion, (FLT[4]){0, 0, precision, 0}); + gradientOut[2] = RotationEstimateFitness(lhPoint, tmp2plus, obj) - baseFitness; + + FLT tmp3plus[4]; + quatadd(tmp3plus, quaternion, (FLT[4]){0, 0, 0, precision}); + gradientOut[3] = RotationEstimateFitness(lhPoint, tmp3plus, obj) - baseFitness; + + return; +} + +void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagnitude) +{ + quatnormalize(vectorToScale, vectorToScale); + quatscale(vectorToScale, vectorToScale, desiredMagnitude); + return; +} + +static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) { int i = 0; - FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount); - Point lastPoint = initialEstimate; + FLT lastMatchFitness = RotationEstimateFitness(lhPoint, initialEstimate, obj); + + quatcopy(rotOut, initialEstimate); // The values below are somewhat magic, and definitely tunable // The initial vlue of g will represent the biggest step that the gradient descent can take at first. @@ -622,23 +723,25 @@ static Point RefineRotationEstimate(Point initialEstimate, PointsAndAngle *pna, for (FLT g = 0.2; g > 0.00001; g *= 0.99) { i++; - Point point1 = lastPoint; + FLT point1[3]; + copy3d(point1, rotOut); // let's get 3 iterations of gradient descent here. - Point gradient1 = getGradient(point1, pna, pnaCount, g / 1000 /*somewhat arbitrary*/); - Point gradientN1 = getNormalizedVector(gradient1, g); + FLT gradient1[4]; + + getRotationGradient(gradient1, lhPoint, point1, obj, g/1000); + getNormalizedAndScaledRotationGradient(gradient1,g); - Point point2; - point2.x = point1.x + gradientN1.x; - point2.y = point1.y + gradientN1.y; - point2.z = point1.z + gradientN1.z; + FLT point2[4]; + quatadd(point2, gradient1, point1); + quatnormalize(point2,point2); - Point gradient2 = getGradient(point2, pna, pnaCount, g / 1000 /*somewhat arbitrary*/); - Point gradientN2 = getNormalizedVector(gradient2, g); + FLT gradient2[4]; + getRotationGradient(gradient2, lhPoint, point2, obj, g/1000); + getNormalizedAndScaledRotationGradient(gradient2,g); - Point point3; - point3.x = point2.x + gradientN2.x; - point3.y = point2.y + gradientN2.y; - point3.z = point2.z + gradientN2.z; + FLT point3[4]; + quatadd(point3, gradient2, point2); + quatnormalize(point3,point3); // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley? // Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic @@ -647,48 +750,41 @@ static Point RefineRotationEstimate(Point initialEstimate, PointsAndAngle *pna, // 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 }; + FLT specialGradient[4]; + quatsub(specialGradient,point3,point1); // The second parameter to this function is very much a tunable parameter. Different values will result // in a different number of iterations before we get to the minimum. Numbers between 3-10 seem to work well // It's not clear what would be optimum here. - specialGradient = getNormalizedVector(specialGradient, g / 4); + getNormalizedAndScaledRotationGradient(specialGradient,g/4); - Point point4; + FLT point4[4]; + quatadd(point4, specialGradient, point3); + quatnormalize(point4,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); + FLT newMatchFitness = RotationEstimateFitness(lhPoint, point4, obj); if (newMatchFitness > lastMatchFitness) { - if (logFile) - { - writePoint(logFile, lastPoint.x, lastPoint.y, lastPoint.z, 0xFFFFFF); - } lastMatchFitness = newMatchFitness; - lastPoint = point4; -#ifdef TORI_DEBUG + quatcopy(rotOut, point4); +//#ifdef TORI_DEBUG printf("+"); -#endif +//#endif } else { -#ifdef TORI_DEBUG +//#ifdef TORI_DEBUG printf("-"); -#endif +//#endif g *= 0.7; } } - printf("\ni=%d\n", i); - - return lastPoint; + printf("\nRi=%d\n", i); } void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) @@ -698,12 +794,14 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) // 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); + FLT zAxis[4] = { 0, 0, 1 ,0}; + //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. + RefineRotationEstimate(rotOut, lh, zAxis, obj); } @@ -767,7 +865,7 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) // intentionally picking the direction of the average normal vector of the sensors that see the lighthouse // since this is least likely to pick the incorrect "mirror" point that would send us // back into the search for the correct point (see "if (a1 > M_PI / 2)" below) - Point p1 = getNormalizedVector(avgNorm, 8); + Point p1 = getNormalizedAndScaledVector(avgNorm, 8); Point refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p1, pna, pnaCount, logFile); @@ -792,6 +890,9 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) printf("(%4.4f, %4.4f, %4.4f)\n", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z); printf("Distance is %f, Fitness is %f\n", distance, fitGd); + FLT rot[4]; + SolveForRotation(rot, obj, refinedEstimateGd); + if (logFile) { updateHeader(logFile); -- cgit v1.2.3 From ad5afae7e264ffa72ed16d2ec2c6f6090e7aa7b5 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 13:02:23 -0700 Subject: Adding operation for rotating about an axis --- redist/linmath.c | 19 +++++++++++++++++++ redist/linmath.h | 1 + 2 files changed, 20 insertions(+) diff --git a/redist/linmath.c b/redist/linmath.c index eefcd5f..0e06156 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -82,6 +82,25 @@ FLT anglebetween3d( FLT * a, FLT * b ) return FLT_ACOS(dot); } +// algorithm found here: http://inside.mines.edu/fs_home/gmurray/ArbitraryAxisRotation/ +void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle) +{ + FLT s = FLT_SIN(angle); + FLT c = FLT_COS(angle); + + FLT u=axis[0]; + FLT v=axis[1]; + FLT w=axis[2]; + + FLT x=invec3[0]; + FLT y=invec3[1]; + FLT z=invec3[2]; + + outvec3[0] = u*(u*x + v*y + w*z)*(1-c) + x*c + (-w*y + v*z)*s; + outvec3[1] = v*(u*x + v*y + w*z)*(1-c) + y*c + ( w*x - u*z)*s; + outvec3[2] = w*(u*x + v*y + w*z)*(1-c) + z*c + (-v*x + u*y)*s; +} + /////////////////////////////////////QUATERNIONS////////////////////////////////////////// //Originally from Mercury (Copyright (C) 2009 by Joshua Allen, Charles Lohr, Adam Lowman) //Under the mit/X11 license. diff --git a/redist/linmath.h b/redist/linmath.h index 4e0cb77..6f0bf60 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -70,6 +70,7 @@ FLT magnitude3d(const FLT * a ); FLT anglebetween3d( FLT * a, FLT * b ); +void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle); //Quaternion things... void quatsetnone( FLT * q ); -- cgit v1.2.3 From 9ead7de95621f1d7d59fed26fc7431344fdd9db4 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 13:58:04 -0700 Subject: Something is converging. --- src/poser_turveytori.c | 29 +++++++++++++++++++---------- 1 file changed, 19 insertions(+), 10 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 15961c8..824fabb 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -646,7 +646,8 @@ FLT RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj) FLT sensor_in_lh_reference_frame[3]; sub3d(sensor_in_lh_reference_frame, sensor_in_obj_reference_frame, (FLT[3]){lhPoint.x, lhPoint.y, lhPoint.z}); - quatrotatevector(sensor_in_lh_reference_frame, quaternion, sensor_in_lh_reference_frame); + //quatrotatevector(sensor_in_lh_reference_frame, quaternion, sensor_in_lh_reference_frame); + rotatearoundaxis(sensor_in_lh_reference_frame, sensor_in_lh_reference_frame, quaternion, quaternion[3]); // now the we've got the location of the sensor in the lighthouses's reference frame, given lhPoint and quaternion inputs. @@ -666,7 +667,7 @@ FLT RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj) fitness = FLT_SQRT(fitness); - return fitness; + return 1/fitness; } void getRotationGradient(FLT *gradientOut, Point lhPoint, FLT *quaternion, TrackedObject *obj, FLT precision) @@ -693,10 +694,18 @@ void getRotationGradient(FLT *gradientOut, Point lhPoint, FLT *quaternion, Track return; } +//void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagnitude) +//{ +// quatnormalize(vectorToScale, vectorToScale); +// quatscale(vectorToScale, vectorToScale, desiredMagnitude); +// return; +//} void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagnitude) { quatnormalize(vectorToScale, vectorToScale); quatscale(vectorToScale, vectorToScale, desiredMagnitude); + //vectorToScale[3] = desiredMagnitude; + return; } @@ -723,8 +732,8 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim for (FLT g = 0.2; g > 0.00001; g *= 0.99) { i++; - FLT point1[3]; - copy3d(point1, rotOut); + FLT point1[4]; + quatcopy(point1, rotOut); // let's get 3 iterations of gradient descent here. FLT gradient1[4]; @@ -733,7 +742,7 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim FLT point2[4]; quatadd(point2, gradient1, point1); - quatnormalize(point2,point2); + //quatnormalize(point2,point2); FLT gradient2[4]; getRotationGradient(gradient2, lhPoint, point2, obj, g/1000); @@ -741,7 +750,7 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim FLT point3[4]; quatadd(point3, gradient2, point2); - quatnormalize(point3,point3); + //quatnormalize(point3,point3); // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley? // Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic @@ -760,7 +769,7 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim FLT point4[4]; quatadd(point4, specialGradient, point3); - quatnormalize(point4,point4); + //quatnormalize(point4,point4); FLT newMatchFitness = RotationEstimateFitness(lhPoint, point4, obj); @@ -770,13 +779,13 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim lastMatchFitness = newMatchFitness; quatcopy(rotOut, point4); //#ifdef TORI_DEBUG - printf("+"); + printf("+ %8.8f, %f\n", newMatchFitness, point4[3]); //#endif } else { //#ifdef TORI_DEBUG - printf("-"); + printf("- , %f\n", point4[3]); //#endif g *= 0.7; @@ -794,7 +803,7 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) // This should have the lighthouse directly facing the tracked object. Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z }; FLT theta = atan2(-lh.x, -lh.y); - FLT zAxis[4] = { 0, 0, 1 ,0}; + FLT zAxis[4] = { 0, 0, 1 , theta}; //FLT quat1[4]; //quatfromaxisangle(quat1, zAxis, theta); // not correcting for phi, but that's less important. -- cgit v1.2.3 From e5c15af3af93356bb056624726e0b6068354690f Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 14:20:29 -0700 Subject: Getting very close --- redist/linmath.c | 3 +++ src/poser_turveytori.c | 17 ++++++++++++++++- 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/redist/linmath.c b/redist/linmath.c index 0e06156..69a70f6 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -85,6 +85,9 @@ FLT anglebetween3d( FLT * a, FLT * b ) // algorithm found here: http://inside.mines.edu/fs_home/gmurray/ArbitraryAxisRotation/ void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle) { + // TODO: this really should be external. + normalize3d(axis, axis); + FLT s = FLT_SIN(angle); FLT c = FLT_COS(angle); diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 824fabb..d737723 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -779,8 +779,10 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim lastMatchFitness = newMatchFitness; quatcopy(rotOut, point4); //#ifdef TORI_DEBUG - printf("+ %8.8f, %f\n", newMatchFitness, point4[3]); + printf("+ %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]); //#endif + g *= 1.03; + } else { @@ -796,6 +798,17 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim printf("\nRi=%d\n", i); } +static void WhereIsTheTrackedObject(FLT *rotation, Point lhPoint) +{ + FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], -rotation[3]}; + FLT objPoint[3] = {lhPoint.x, lhPoint.y, lhPoint.z}; + + rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); + + printf("The tracked object is at location (%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); +} + + void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) { @@ -812,6 +825,8 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) // Step 2, optimize the quaternion to match the data. RefineRotationEstimate(rotOut, lh, zAxis, obj); + WhereIsTheTrackedObject(rotOut, lh); + } -- cgit v1.2.3 From e557b5128a32c99dcda0e6a9784a507258342301 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 14:36:54 -0700 Subject: Small tweak. Using axis/angle --- src/poser_turveytori.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index d737723..62deccf 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -729,7 +729,7 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim // 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) + for (FLT g = 0.2; g > 0.000000001; g *= 0.99) { i++; FLT point1[4]; @@ -781,7 +781,7 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim //#ifdef TORI_DEBUG printf("+ %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]); //#endif - g *= 1.03; + g *= 1.02; } else -- cgit v1.2.3 From f86a7a9927c484f46768b14a5944a03863beee34 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 15:30:54 -0700 Subject: sucky angle estimators (quat & angle axis) --- src/poser_turveytori.c | 265 ++++++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 242 insertions(+), 23 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 62deccf..da64850 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -602,7 +602,7 @@ FLT RotationEstimateFitnessOld(Point lhPoint, FLT *quaternion, TrackedObject *ob // 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) +FLT RotationEstimateFitnessAxisAngle(Point lhPoint, FLT *quaternion, TrackedObject *obj) { FLT fitness = 0; for (size_t i = 0; i < obj->numSensors; i++) @@ -670,26 +670,121 @@ FLT RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj) return 1/fitness; } -void getRotationGradient(FLT *gradientOut, Point lhPoint, FLT *quaternion, TrackedObject *obj, FLT precision) +// interesting-- this is one place where we could use any sensors that are only hit by +// just an x or y axis to make our estimate better. TODO: bring that data to this fn. +FLT RotationEstimateFitnessQuaternion(Point lhPoint, FLT *quaternion, TrackedObject *obj) { + FLT fitness = 0; + for (size_t i = 0; i < obj->numSensors; i++) + { + // first, get the normal of the plane for the horizonal sweep + FLT theta = obj->sensor[i].theta; + // make two vectors that lie on the plane + FLT t1H[3] = { 1, tan(theta-LINMATHPI/2), 0 }; + FLT t2H[3] = { 1, tan(theta-LINMATHPI/2), 1 }; + + FLT tNormH[3]; + + // the normal is the cross of two vectors on the plane. + cross3d(tNormH, t1H, t2H); + + normalize3d(tNormH, tNormH); + + // Now do the same for the vertical sweep + + // first, get the normal of the plane for the horizonal sweep + FLT phi = obj->sensor[i].phi; + // make two vectors that lie on the plane + FLT t1V[3] = { 0, 1, tan(phi-LINMATHPI/2)}; + FLT t2V[3] = { 1, 1, tan(phi-LINMATHPI/2)}; + + FLT tNormV[3]; + + // the normal is the cross of two vectors on the plane. + cross3d(tNormV, t1V, t2V); + + normalize3d(tNormV, tNormV); + + + // First, where is the sensor in the object's reference frame? + FLT sensor_in_obj_reference_frame[3] = {obj->sensor->point.x, obj->sensor->point.y, obj->sensor->point.z}; + // Where is the point, in the reference frame of the lighthouse? + // This has two steps, first we translate from the object's location being the + // origin to the lighthouse being the origin. + // And second, we apply the quaternion to rotate into the proper reference frame for the lighthouse. + + FLT sensor_in_lh_reference_frame[3]; + sub3d(sensor_in_lh_reference_frame, sensor_in_obj_reference_frame, (FLT[3]){lhPoint.x, lhPoint.y, lhPoint.z}); + + quatrotatevector(sensor_in_lh_reference_frame, quaternion, sensor_in_lh_reference_frame); + //rotatearoundaxis(sensor_in_lh_reference_frame, sensor_in_lh_reference_frame, quaternion, quaternion[3]); - FLT baseFitness = RotationEstimateFitness(lhPoint, quaternion, obj); + // now the we've got the location of the sensor in the lighthouses's reference frame, given lhPoint and quaternion inputs. + + // We need an arbitrary vector from the plane to the point. + // Since the plane goes through the origin, this is trivial. + // The sensor point itself is such a vector! + + // And go calculate the distances! + // TODO: don't need to ABS these because we square them below. + FLT dH = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormH)); + FLT dV = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormV)); + + + fitness += SQUARED(dH); + fitness += SQUARED(dV); + } + + fitness = FLT_SQRT(fitness); + + return 1/fitness; +} + + +void getRotationGradientQuaternion(FLT *gradientOut, Point lhPoint, FLT *quaternion, TrackedObject *obj, FLT precision) +{ + + FLT baseFitness = RotationEstimateFitnessQuaternion(lhPoint, quaternion, obj); FLT tmp0plus[4]; quatadd(tmp0plus, quaternion, (FLT[4]){precision, 0, 0, 0}); - gradientOut[0] = RotationEstimateFitness(lhPoint, tmp0plus, obj) - baseFitness; + gradientOut[0] = RotationEstimateFitnessQuaternion(lhPoint, tmp0plus, obj) - baseFitness; FLT tmp1plus[4]; quatadd(tmp1plus, quaternion, (FLT[4]){0, precision, 0, 0}); - gradientOut[1] = RotationEstimateFitness(lhPoint, tmp1plus, obj) - baseFitness; + gradientOut[1] = RotationEstimateFitnessQuaternion(lhPoint, tmp1plus, obj) - baseFitness; FLT tmp2plus[4]; quatadd(tmp2plus, quaternion, (FLT[4]){0, 0, precision, 0}); - gradientOut[2] = RotationEstimateFitness(lhPoint, tmp2plus, obj) - baseFitness; + gradientOut[2] = RotationEstimateFitnessQuaternion(lhPoint, tmp2plus, obj) - baseFitness; FLT tmp3plus[4]; quatadd(tmp3plus, quaternion, (FLT[4]){0, 0, 0, precision}); - gradientOut[3] = RotationEstimateFitness(lhPoint, tmp3plus, obj) - baseFitness; + gradientOut[3] = RotationEstimateFitnessQuaternion(lhPoint, tmp3plus, obj) - baseFitness; + + return; +} + +void getRotationGradientAxisAngle(FLT *gradientOut, Point lhPoint, FLT *quaternion, TrackedObject *obj, FLT precision) +{ + + FLT baseFitness = RotationEstimateFitnessAxisAngle(lhPoint, quaternion, obj); + + FLT tmp0plus[4]; + quatadd(tmp0plus, quaternion, (FLT[4]){precision, 0, 0, 0}); + gradientOut[0] = RotationEstimateFitnessAxisAngle(lhPoint, tmp0plus, obj) - baseFitness; + + FLT tmp1plus[4]; + quatadd(tmp1plus, quaternion, (FLT[4]){0, precision, 0, 0}); + gradientOut[1] = RotationEstimateFitnessAxisAngle(lhPoint, tmp1plus, obj) - baseFitness; + + FLT tmp2plus[4]; + quatadd(tmp2plus, quaternion, (FLT[4]){0, 0, precision, 0}); + gradientOut[2] = RotationEstimateFitnessAxisAngle(lhPoint, tmp2plus, obj) - baseFitness; + + FLT tmp3plus[4]; + quatadd(tmp3plus, quaternion, (FLT[4]){0, 0, 0, precision}); + gradientOut[3] = RotationEstimateFitnessAxisAngle(lhPoint, tmp3plus, obj) - baseFitness; return; } @@ -709,10 +804,20 @@ void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagni return; } -static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) +static void WhereIsTheTrackedObjectAxisAngle(FLT *rotation, Point lhPoint) +{ + FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], -rotation[3]}; + FLT objPoint[3] = {lhPoint.x, lhPoint.y, lhPoint.z}; + + rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); + + printf("The tracked object is at location (%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); +} + +static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) { int i = 0; - FLT lastMatchFitness = RotationEstimateFitness(lhPoint, initialEstimate, obj); + FLT lastMatchFitness = RotationEstimateFitnessAxisAngle(lhPoint, initialEstimate, obj); quatcopy(rotOut, initialEstimate); @@ -729,7 +834,7 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim // 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.000000001; g *= 0.99) + for (FLT g = 0.1; g > 0.000000001; g *= 0.99) { i++; FLT point1[4]; @@ -737,19 +842,26 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim // let's get 3 iterations of gradient descent here. FLT gradient1[4]; - getRotationGradient(gradient1, lhPoint, point1, obj, g/1000); + normalize3d(point1, point1); + + getRotationGradientAxisAngle(gradient1, lhPoint, point1, obj, g/10000); getNormalizedAndScaledRotationGradient(gradient1,g); FLT point2[4]; quatadd(point2, gradient1, point1); //quatnormalize(point2,point2); + normalize3d(point1, point1); + FLT gradient2[4]; - getRotationGradient(gradient2, lhPoint, point2, obj, g/1000); + getRotationGradientAxisAngle(gradient2, lhPoint, point2, obj, g/10000); getNormalizedAndScaledRotationGradient(gradient2,g); FLT point3[4]; quatadd(point3, gradient2, point2); + + normalize3d(point1, point1); + //quatnormalize(point3,point3); // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley? @@ -770,8 +882,9 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim FLT point4[4]; quatadd(point4, specialGradient, point3); //quatnormalize(point4,point4); + normalize3d(point1, point1); - FLT newMatchFitness = RotationEstimateFitness(lhPoint, point4, obj); + FLT newMatchFitness = RotationEstimateFitnessAxisAngle(lhPoint, point4, obj); if (newMatchFitness > lastMatchFitness) { @@ -797,18 +910,117 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim } printf("\nRi=%d\n", i); } - -static void WhereIsTheTrackedObject(FLT *rotation, Point lhPoint) +static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint) { FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], -rotation[3]}; FLT objPoint[3] = {lhPoint.x, lhPoint.y, lhPoint.z}; - rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); - + //rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); + quatrotatevector(objPoint, rotation, objPoint); printf("The tracked object is at location (%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); } + +static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) +{ + int i = 0; + FLT lastMatchFitness = RotationEstimateFitnessQuaternion(lhPoint, initialEstimate, obj); + + quatcopy(rotOut, initialEstimate); + + // The values below are somewhat magic, and definitely tunable + // The initial vlue of g will represent the biggest step that the gradient descent can take at first. + // bigger values may be faster, especially when the initial guess is wildly off. + // The downside to a bigger starting guess is that if we've picked a good guess at the local minima + // if there are other local minima, we may accidentally jump to such a local minima and get stuck there. + // That's fairly unlikely with the lighthouse problem, from expereince. + // The other downside is that if it's too big, we may have to spend a few iterations before it gets down + // to a size that doesn't jump us out of our minima. + // The terminal value of g represents how close we want to get to the local minima before we're "done" + // The change in value of g for each iteration is intentionally very close to 1. + // in fact, it probably could probably be 1 without any issue. The main place where g is decremented + // is in the block below when we've made a jump that results in a worse fitness than we're starting at. + // In those cases, we don't take the jump, and instead lower the value of g and try again. + for (FLT g = 0.1; g > 0.000000001; g *= 0.99) + { + i++; + FLT point1[4]; + quatcopy(point1, rotOut); + // let's get 3 iterations of gradient descent here. + FLT gradient1[4]; + + //normalize3d(point1, point1); + + getRotationGradientQuaternion(gradient1, lhPoint, point1, obj, g/10000); + getNormalizedAndScaledRotationGradient(gradient1,g); + + FLT point2[4]; + quatadd(point2, gradient1, point1); + quatnormalize(point2,point2); + + //normalize3d(point1, point1); + + FLT gradient2[4]; + getRotationGradientQuaternion(gradient2, lhPoint, point2, obj, g/10000); + getNormalizedAndScaledRotationGradient(gradient2,g); + + FLT point3[4]; + quatadd(point3, gradient2, point2); + + //normalize3d(point1, point1); + + quatnormalize(point3,point3); + + // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley? + // Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic + // gradient descent is kinda horrible here. Instead, think about the shape that a zig-zagging + // converging gradient descent makes. Instead of using the gradient as the best indicator of + // the direction we should follow, we're looking at one side of the zig-zag pattern, and specifically + // following *that* vector. As it turns out, this works *amazingly* well. + + FLT specialGradient[4]; + quatsub(specialGradient,point3,point1); + + // The second parameter to this function is very much a tunable parameter. Different values will result + // in a different number of iterations before we get to the minimum. Numbers between 3-10 seem to work well + // It's not clear what would be optimum here. + getNormalizedAndScaledRotationGradient(specialGradient,g/4); + + FLT point4[4]; + quatadd(point4, specialGradient, point3); + quatnormalize(point4,point4); + //normalize3d(point1, point1); + + FLT newMatchFitness = RotationEstimateFitnessQuaternion(lhPoint, point4, obj); + + if (newMatchFitness > lastMatchFitness) + { + + lastMatchFitness = newMatchFitness; + quatcopy(rotOut, point4); +//#ifdef TORI_DEBUG + //printf("+ %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]); +//#endif + g *= 1.02; + printf("+"); + WhereIsTheTrackedObjectQuaternion(rotOut, lhPoint); + } + else + { +//#ifdef TORI_DEBUG + //printf("- , %f\n", point4[3]); +//#endif + g *= 0.7; + printf("-"); + } + + + } + printf("\nRi=%d Fitness=%3f\n", i, lastMatchFitness); +} + + void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) { @@ -816,16 +1028,23 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) // This should have the lighthouse directly facing the tracked object. Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z }; FLT theta = atan2(-lh.x, -lh.y); - FLT zAxis[4] = { 0, 0, 1 , theta}; - //FLT quat1[4]; - //quatfromaxisangle(quat1, zAxis, theta); + FLT zAxis[4] = { 0, 0, 1 , theta-LINMATHPI/2}; + FLT quat1[4]; + quatfromaxisangle(quat1, zAxis, theta); + + //quatfrom2vectors(0,0) // not correcting for phi, but that's less important. - // Step 2, optimize the quaternion to match the data. - RefineRotationEstimate(rotOut, lh, zAxis, obj); + // Step 2, optimize the axis/ angle to match the data. + RefineRotationEstimateAxisAngle(rotOut, lh, zAxis, obj); + + WhereIsTheTrackedObjectAxisAngle(rotOut, lh); + + //// Step 2, optimize the quaternion to match the data. + //RefineRotationEstimateQuaternion(rotOut, lh, quat1, obj); - WhereIsTheTrackedObject(rotOut, lh); + //WhereIsTheTrackedObjectQuaternion(rotOut, lh); } -- cgit v1.2.3 From d8c4b23789fd3aedb150144bed6beb286d1504a9 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 16:21:52 -0700 Subject: Reliable detection of orientation of lighthouse Lots of cruft. Lots of room for improving performance. --- src/poser_turveytori.c | 44 +++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 41 insertions(+), 3 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index da64850..5c3350b 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -600,9 +600,47 @@ FLT RotationEstimateFitnessOld(Point lhPoint, FLT *quaternion, TrackedObject *ob return fitness; } +FLT RotationEstimateFitnessAxisAngle(Point lh, FLT *AxisAngle, TrackedObject *obj) +{ + // For this fitness calculator, we're going to use the rotation information to figure out where + // we expect to see the tracked object sensors, and we'll do a sum of squares to grade + // the quality of the guess formed by the AxisAngle; + + FLT fitness = 0; + + // for each point in the tracked object + for (int i=0; i< obj->numSensors; i++) + { + + + + // let's see... we need to figure out where this sensor should be in the LH reference frame. + FLT sensorLocation[3] = {obj->sensor[i].point.x-lh.x, obj->sensor[i].point.y-lh.y, obj->sensor[i].point.z-lh.z}; + + // And this puppy needs to be rotated... + + rotatearoundaxis(sensorLocation, sensorLocation, AxisAngle, AxisAngle[3]); + + // Now, the vector indicating the position of the sensor, as seen by the lighthouse is: + FLT realVectFromLh[3] = {1, tan(obj->sensor[i].theta - LINMATHPI/2), tan(obj->sensor[i].phi - LINMATHPI/2)}; + + // and the vector we're calculating given the rotation passed in is the same as the sensor location: + FLT calcVectFromLh[3] = {sensorLocation[0], sensorLocation[1], sensorLocation[2]}; + + FLT angleBetween = anglebetween3d( realVectFromLh, calcVectFromLh ); + + fitness += SQUARED(angleBetween); + } + + return 1/FLT_SQRT(fitness); +} + +// This figures out how far away from the scanned planes each point is, then does a sum of squares +// for the fitness. +// // interesting-- this is one place where we could use any sensors that are only hit by // just an x or y axis to make our estimate better. TODO: bring that data to this fn. -FLT RotationEstimateFitnessAxisAngle(Point lhPoint, FLT *quaternion, TrackedObject *obj) +FLT RotationEstimateFitnessAxisAngleOriginal(Point lhPoint, FLT *quaternion, TrackedObject *obj) { FLT fitness = 0; for (size_t i = 0; i < obj->numSensors; i++) @@ -892,7 +930,7 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini lastMatchFitness = newMatchFitness; quatcopy(rotOut, point4); //#ifdef TORI_DEBUG - printf("+ %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]); + //printf("+ %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]); //#endif g *= 1.02; @@ -900,7 +938,7 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini else { //#ifdef TORI_DEBUG - printf("- , %f\n", point4[3]); + //printf("- , %f\n", point4[3]); //#endif g *= 0.7; -- cgit v1.2.3 From 788816a1c12499da9e419bde1978c056fe48cd4b Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 16:36:39 -0700 Subject: Adding sample config file --- useful_files/sample.config.json | 8 ++++++++ 1 file changed, 8 insertions(+) create mode 100644 useful_files/sample.config.json diff --git a/useful_files/sample.config.json b/useful_files/sample.config.json new file mode 100644 index 0000000..20a8f0c --- /dev/null +++ b/useful_files/sample.config.json @@ -0,0 +1,8 @@ +"_Comment0":"This file must be named config.json and placed in the same directory as the executable" +"_Comment1":"Valid Posers Are: PoserCharlesSlow, PoserDaveOrtho, PoserDummy, PoserOctavioRadii, PoserTurveyTori", +"DefaultPoser":"PoserTurveyTori", +"ConfigPoser":"PoserTurveyTori" +"_Comment2":"RequiredTrackersForCal takes a comma separated list of devices required for calibration to pass. Valid options are: HMD,WM0,WM1,TR0,WW0" +"RequiredTrackersForCal":"", +"_Comment3":"If set, AllowAllTrackersForCal will use all trackers for calibration. Otherwise only required trackers will be used." +"AllowAllTrackersForCal":"1", -- cgit v1.2.3 From 94795a17d2d108942d0326e95a1e59e61ab73340 Mon Sep 17 00:00:00 2001 From: Michael Turvey Date: Mon, 27 Mar 2017 16:39:14 -0700 Subject: Update sample.config.json --- useful_files/sample.config.json | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/useful_files/sample.config.json b/useful_files/sample.config.json index 20a8f0c..e12594a 100644 --- a/useful_files/sample.config.json +++ b/useful_files/sample.config.json @@ -1,8 +1,8 @@ -"_Comment0":"This file must be named config.json and placed in the same directory as the executable" +"_Comment0":"This file must be named config.json and placed in the same directory as the executable", "_Comment1":"Valid Posers Are: PoserCharlesSlow, PoserDaveOrtho, PoserDummy, PoserOctavioRadii, PoserTurveyTori", "DefaultPoser":"PoserTurveyTori", -"ConfigPoser":"PoserTurveyTori" -"_Comment2":"RequiredTrackersForCal takes a comma separated list of devices required for calibration to pass. Valid options are: HMD,WM0,WM1,TR0,WW0" +"ConfigPoser":"PoserTurveyTori", +"_Comment2":"RequiredTrackersForCal takes a comma separated list of devices required for calibration to pass. Valid options are: HMD,WM0,WM1,TR0,WW0", "RequiredTrackersForCal":"", -"_Comment3":"If set, AllowAllTrackersForCal will use all trackers for calibration. Otherwise only required trackers will be used." +"_Comment3":"If set, AllowAllTrackersForCal will use all trackers for calibration. Otherwise only required trackers will be used.", "AllowAllTrackersForCal":"1", -- cgit v1.2.3 From 6469d979046bf40c4ee76b89d3fc8311dcb16d33 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Mon, 27 Mar 2017 23:17:05 -0700 Subject: Use IMU to orient world coordinate system --- src/poser_turveytori.c | 80 ++++++++++++++++++++++++++++++++++---------------- 1 file changed, 54 insertions(+), 26 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 5c3350b..5db2bc4 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -71,6 +71,7 @@ static const float DefaultPointsPerOuterDiameter = 60; typedef struct { + FLT down[3]; // populated by the IMU for posing int something; //Stuff } ToriData; @@ -1194,23 +1195,32 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) -int PoserTurveyTori( SurviveObject * so, PoserData * pd ) +int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) { - PoserType pt = pd->pt; + PoserType pt = poserData->pt; SurviveContext * ctx = so->ctx; - ToriData * dd = so->PoserData; + ToriData * pd = so->PoserData; - if (!dd) + + if (!pd) { - so->PoserData = dd = malloc(sizeof(ToriData)); - memset(dd, 0, sizeof(ToriData)); + so->PoserData = pd = malloc(sizeof(ToriData)); + memset(pd, 0, sizeof(ToriData)); } switch( pt ) { case POSERDATA_IMU: { - PoserDataIMU * imu = (PoserDataIMU*)pd; + PoserDataIMU * tmpImu = (PoserDataIMU*)pd; + + // store off data we can use for figuring out what direction is down when doing calibration. + if (tmpImu->datamask & 1) // accelerometer data is present + { + pd->down[0] = pd->down[0] * 0.98 + 0.2 * tmpImu->accel[0]; + pd->down[1] = pd->down[1] * 0.98 + 0.2 * tmpImu->accel[1]; + pd->down[2] = pd->down[2] * 0.98 + 0.2 * tmpImu->accel[2]; + } //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; } @@ -1228,30 +1238,40 @@ int PoserTurveyTori( SurviveObject * so, PoserData * pd ) to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor))); - //FLT lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; - //FLT angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; //2 Axes (Angles in LH space) - //FLT synctimes[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES]; + // if we rotate the internal reference frame of of the tracked object from having -z being arbitrary + // to being the down direction as defined by the accelerometer, then when we have come up + // with world coordinate system, it will have Z oriented correctly. + + // let's get the quaternion that represents this rotation. + FLT downQuat[4]; + FLT negZ[3] = { 0,0,-1 }; + quatfrom2vectors(downQuat, negZ, pd->down); - //to->numSensors = so->nr_locations; { int sensorCount = 0; + for (int i = 0; i < so->nr_locations; i++) { if (fs->lengths[i][0][0] != -1 && fs->lengths[i][0][1] != -1) //lh 0 { - to->sensor[sensorCount].normal.x = so->sensor_normals[i * 3 + 0]; - to->sensor[sensorCount].normal.y = so->sensor_normals[i * 3 + 1]; - to->sensor[sensorCount].normal.z = so->sensor_normals[i * 3 + 2]; - to->sensor[sensorCount].point.x = so->sensor_locations[i * 3 + 0]; - to->sensor[sensorCount].point.y = so->sensor_locations[i * 3 + 1]; - to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2]; + FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; + FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; + + quatrotatevector(norm, downQuat, norm); + quatrotatevector(point, downQuat, point); + + to->sensor[sensorCount].normal.x = norm[0]; + to->sensor[sensorCount].normal.y = norm[1]; + to->sensor[sensorCount].normal.z = norm[2]; + to->sensor[sensorCount].point.x = point[0]; + to->sensor[sensorCount].point.y = point[1]; + to->sensor[sensorCount].point.z = point[2]; to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) sensorCount++; } } - to->numSensors = sensorCount; SolveForLighthouse(to, 0); @@ -1264,12 +1284,18 @@ int PoserTurveyTori( SurviveObject * so, PoserData * pd ) { if (fs->lengths[i][lh][0] != -1 && fs->lengths[i][lh][1] != -1) { - to->sensor[sensorCount].normal.x = so->sensor_normals[i * 3 + 0]; - to->sensor[sensorCount].normal.y = so->sensor_normals[i * 3 + 1]; - to->sensor[sensorCount].normal.z = so->sensor_normals[i * 3 + 2]; - to->sensor[sensorCount].point.x = so->sensor_locations[i * 3 + 0]; - to->sensor[sensorCount].point.y = so->sensor_locations[i * 3 + 1]; - to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2]; + FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; + FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; + + quatrotatevector(norm, downQuat, norm); + quatrotatevector(point, downQuat, point); + + to->sensor[sensorCount].normal.x = norm[0]; + to->sensor[sensorCount].normal.y = norm[1]; + to->sensor[sensorCount].normal.z = norm[2]; + to->sensor[sensorCount].point.x = point[0]; + to->sensor[sensorCount].point.y = point[1]; + to->sensor[sensorCount].point.z = point[2]; to->sensor[sensorCount].theta = fs->angles[i][lh][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) to->sensor[sensorCount].phi = fs->angles[i][lh][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) sensorCount++; @@ -1280,13 +1306,15 @@ int PoserTurveyTori( SurviveObject * so, PoserData * pd ) SolveForLighthouse(to, 0); } + + free(to); //printf( "Full scene data.\n" ); break; } case POSERDATA_DISASSOCIATE: { - free( dd ); - so->PoserData = 0; + free( pd ); + so->PoserData = NULL; //printf( "Need to disassociate.\n" ); break; } -- cgit v1.2.3 From b3bdcdb838ed57986f359b2181a624bfd1acbbf1 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Tue, 28 Mar 2017 10:15:36 -0700 Subject: Fix Tracker IMU --- src/poser_turveytori.c | 2 +- src/survive_data.c | 10 ++++++++-- src/survive_vive.c | 17 +---------------- 3 files changed, 10 insertions(+), 19 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 5c3350b..df691e3 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1211,7 +1211,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * pd ) 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] ); + 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: diff --git a/src/survive_data.c b/src/survive_data.c index 9447104..3eb5890 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -154,8 +154,14 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) { if (lcd->per_sweep.activeLighthouse != -1) { - // hmm, it appears we got two non-skip pulses at the same time. That should never happen - fprintf(stderr, "WARNING: Two non-skip pulses received on the same cycle!\n"); + static int pulseWarningCount=0; + + if (pulseWarningCount < 5) + { + pulseWarningCount++; + // hmm, it appears we got two non-skip pulses at the same time. That should never happen + fprintf(stderr, "WARNING: Two non-skip pulses received on the same cycle!\n"); + } } lcd->per_sweep.activeLighthouse = 1; lcd->per_sweep.activeSweepStartTime = le->timestamp; diff --git a/src/survive_vive.c b/src/survive_vive.c index a5c731d..757fa11 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1105,6 +1105,7 @@ void survive_data_cb( SurviveUSBInterface * si ) } case USB_IF_LIGHTHOUSE: case USB_IF_W_WATCHMAN1: + case USB_IF_TRACKER0: { int i; //printf( "%d -> ", size ); @@ -1156,22 +1157,6 @@ void survive_data_cb( SurviveUSBInterface * si ) } break; } - case USB_IF_TRACKER0: - { - SurviveObject * w = obj; - if( id == 32 ) - { - // TODO: Looks like this will need to be handle_tracker, since - // it appears the interface is sufficiently different. - // More work needd to reverse engineer it. - //handle_wired_watchman( w, readdata, size); - } - else - { - SV_INFO( "Unknown tracker code %d\n", id ); - } - break; - } case USB_IF_LIGHTCAP: { int i; -- cgit v1.2.3 From fba18d9de738fd07a0b6db944369127a6a66f0d8 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Tue, 28 Mar 2017 11:19:39 -0700 Subject: Add lh to poser light data --- include/libsurvive/poser.h | 3 ++- src/poser_turveytori.c | 38 ++++++++++++++++++++++++-------------- src/survive_process.c | 1 + 3 files changed, 27 insertions(+), 15 deletions(-) diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h index cf11e0c..497b009 100644 --- a/include/libsurvive/poser.h +++ b/include/libsurvive/poser.h @@ -32,7 +32,8 @@ typedef struct { PoserType pt; int sensor_id; - int acode; //OOTX Code associated with this sweep. base_station = acode >> 2; axis = acode & 1; + int acode; //OOTX Code associated with this sweep. bit 1 indicates vertical(1) or horizontal(0) sweep + int lh; //Lighthouse making this sweep uint32_t timecode; //In object-local ticks. FLT length; //In seconds FLT angle; //In radians from center of lighthouse. diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 177a16a..8b92860 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1199,42 +1199,51 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) { PoserType pt = poserData->pt; SurviveContext * ctx = so->ctx; - ToriData * pd = so->PoserData; + ToriData * td = so->PoserData; - if (!pd) + if (!td) { - so->PoserData = pd = malloc(sizeof(ToriData)); - memset(pd, 0, sizeof(ToriData)); + so->PoserData = td = malloc(sizeof(ToriData)); + memset(td, 0, sizeof(ToriData)); } switch( pt ) { case POSERDATA_IMU: { - PoserDataIMU * tmpImu = (PoserDataIMU*)pd; + PoserDataIMU * tmpImu = (PoserDataIMU*)poserData; // store off data we can use for figuring out what direction is down when doing calibration. - if (tmpImu->datamask & 1) // accelerometer data is present + //TODO: looks like the data mask isn't getting set correctly. + //if (tmpImu->datamask & 1) // accelerometer data is present { - pd->down[0] = pd->down[0] * 0.98 + 0.2 * tmpImu->accel[0]; - pd->down[1] = pd->down[1] * 0.98 + 0.2 * tmpImu->accel[1]; - pd->down[2] = pd->down[2] * 0.98 + 0.2 * tmpImu->accel[2]; + td->down[0] = td->down[0] * 0.98 + 0.02 * tmpImu->accel[0]; + td->down[1] = td->down[1] * 0.98 + 0.02 * tmpImu->accel[1]; + td->down[2] = td->down[2] * 0.98 + 0.02 * tmpImu->accel[2]; } - printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, imu->accel[0], imu->accel[1], imu->accel[2], imu->gyro[0], imu->gyro[1], imu->gyro[2] ); + //printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, tmpImu->accel[0], tmpImu->accel[1], tmpImu->accel[2], tmpImu->gyro[0], tmpImu->gyro[1], tmpImu->gyro[2] ); + //printf( "Down: (%f %f %f)\n", td->down[0], td->down[1], td->down[2] ); break; } case POSERDATA_LIGHT: { - PoserDataLight * l = (PoserDataLight*)pd; + PoserDataLight * l = (PoserDataLight*)poserData; //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode ); + if (0 == l->lh) + { + if (l->acode & 0x1) + { + printf("%2d: %8f\n", l->sensor_id, l->angle); + } + } break; } case POSERDATA_FULL_SCENE: { TrackedObject *to; - PoserDataFullScene * fs = (PoserDataFullScene*)pd; + PoserDataFullScene * fs = (PoserDataFullScene*)poserData; to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor))); @@ -1245,7 +1254,8 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) // let's get the quaternion that represents this rotation. FLT downQuat[4]; FLT negZ[3] = { 0,0,-1 }; - quatfrom2vectors(downQuat, negZ, pd->down); + //quatfrom2vectors(downQuat, negZ, td->down); + quatfrom2vectors(downQuat, td->down, negZ); { int sensorCount = 0; @@ -1313,7 +1323,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) } case POSERDATA_DISASSOCIATE: { - free( pd ); + free( so->PoserData ); so->PoserData = NULL; //printf( "Need to disassociate.\n" ); break; diff --git a/src/survive_process.c b/src/survive_process.c index b58b344..3af2da9 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -57,6 +57,7 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode .timecode = timecode, .length = length, .angle = angle, + .lh = lh, }; so->PoserFn( so, (PoserData *)&l ); } -- cgit v1.2.3 From 3ca8ba3d69de226ae8835bb29e45c7bcd35793fe Mon Sep 17 00:00:00 2001 From: mwturvey Date: Wed, 29 Mar 2017 16:39:47 -0700 Subject: Tori Poser Works! There's a ton of code cruft, and the algorithm is currently too slow. BUT I can track an object using only 1 lighthouse for tracking, at (I believe) an update rate of at least 7.5 HZ. By tracking, I know the position and orientation of the lighthouses relative to the tracked object, and I know the tracked object's location relative to the lighthouse. I don't have the orientation of the tracked object relative to the lighthouse yet, but that should be easy given the rest of the "knowns." --- src/poser_octavioradii.c | 198 +++++++++++++++++++++++++++++++++++++++++++++-- src/poser_turveytori.c | 174 +++++++++++++++++++++++++++++++++++++---- src/survive_data.c | 37 ++++++++- 3 files changed, 381 insertions(+), 28 deletions(-) diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c index 3893085..0d8674c 100644 --- a/src/poser_octavioradii.c +++ b/src/poser_octavioradii.c @@ -4,8 +4,12 @@ typedef struct { - int something; - //Stuff +#define OLD_ANGLES_BUFF_LEN 3 + FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance + int angleIndex[NUM_LIGHTHOUSES][2]; // index into circular buffer ahead. separate index for each axis. + int lastAxis[NUM_LIGHTHOUSES]; + + int hitCount[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; } OctavioRadiiData; #include @@ -45,6 +49,7 @@ typedef struct Point normal; // unit vector indicating the normal for the sensor double theta; // "horizontal" angular measurement from lighthouse radians double phi; // "vertical" angular measurement from lighthouse in radians. + int id; } TrackedSensor; typedef struct @@ -382,10 +387,10 @@ static RefineEstimateUsingGradientDescentRadii(FLT *estimateOut, SensorAngles *a } - printf("\ni=%d\n", i); + printf(" i=%d ", i); } -void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObject *obj) +static void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObject *obj) { FLT estimate[MAX_RADII]; @@ -394,6 +399,12 @@ void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObj estimate[i] = 2.38; } + + //for (int i=0; i < obj->numSensors; i++) + //{ + // printf("%d, ", obj->sensor[i].id); + //} + SensorAngles angles[MAX_RADII]; PointPair pairs[MAX_POINT_PAIRS]; @@ -423,22 +434,119 @@ void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObj // we should now have an estimate of the radii. - for (int i = 0; i < obj->numSensors; i++) + //for (int i = 0; i < obj->numSensors; i++) + for (int i = 0; i < 1; i++) { printf("radius[%d]: %f\n", i, estimate[i]); } + // (FLT *estimateOut, SensorAngles *angles, FLT *initialEstimate, size_t numRadii, PointPair *pairs, size_t numPairs, FILE *logFile) return; } +static void QuickPose(SurviveObject *so) +{ + OctavioRadiiData * td = so->PoserData; + + + //for (int i=0; i < so->nr_locations; i++) + //{ + // FLT x0=td->oldAngles[i][0][0][td->angleIndex[0][0]]; + // FLT y0=td->oldAngles[i][1][0][td->angleIndex[0][1]]; + // //FLT x1=td->oldAngles[i][0][1][td->angleIndex[1][0]]; + // //FLT y1=td->oldAngles[i][1][1][td->angleIndex[1][1]]; + // //printf("%2d: %8.8f, %8.8f %8.8f, %8.8f \n", + // // i, + // // x0, + // // y0, + // // x1, + // // y1 + // // ); + // printf("%2d: %8.8f, %8.8f \n", + // i, + // x0, + // y0 + // ); + //} + //printf("\n"); + + TrackedObject *to; + + to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor))); + + { + int sensorCount = 0; + + for (int i = 0; i < so->nr_locations; i++) + { + int lh = 0; + //printf("%d[%d], ",i,td->hitCount[i][lh][0]); + + int angleIndex0 = (td->angleIndex[lh][0] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN; + int angleIndex1 = (td->angleIndex[lh][1] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN; + if ((td->oldAngles[i][0][lh][angleIndex0] != 0 && td->oldAngles[i][1][lh][angleIndex1] != 0)) + + + { + if (td->hitCount[i][lh][0] > 10 && td->hitCount[i][lh][1] > 10) + { + FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; + FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; + + to->sensor[sensorCount].normal.x = norm[0]; + to->sensor[sensorCount].normal.y = norm[1]; + to->sensor[sensorCount].normal.z = norm[2]; + to->sensor[sensorCount].point.x = point[0]; + to->sensor[sensorCount].point.y = point[1]; + to->sensor[sensorCount].point.z = point[2]; + to->sensor[sensorCount].theta = td->oldAngles[i][0][lh][angleIndex0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = td->oldAngles[i][1][lh][angleIndex1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) + to->sensor[sensorCount].id=i; + + + + //printf("%2d: %8.8f, %8.8f \n", + // i, + // to->sensor[sensorCount].theta, + // to->sensor[sensorCount].phi + // ); + + sensorCount++; + } + } + } + //printf("\n"); + to->numSensors = sensorCount; + + if (sensorCount > 4) + { + FLT pos[3]; + FLT orient[4]; + SolveForLighthouseRadii(pos, orient, to); + } + + + } + + + free(to); + +} + + int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) { PoserType pt = pd->pt; SurviveContext * ctx = so->ctx; OctavioRadiiData * dd = so->PoserData; - if( !dd ) so->PoserData = dd = malloc( sizeof(OctavioRadiiData) ); + if( !dd ) + { + so->PoserData = dd = malloc( sizeof(OctavioRadiiData) ); + memset(dd, 0, sizeof(OctavioRadiiData)); + } + switch( pt ) { @@ -451,9 +559,81 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) case POSERDATA_LIGHT: { PoserDataLight * l = (PoserDataLight*)pd; + + if (l->lh >= NUM_LIGHTHOUSES || l->lh < 0) + { + // should never happen. Famous last words... + break; + } + int axis = l->acode & 0x1; + + //printf("%d ", l->sensor_id); + + //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode ); - break; - } + if ((dd->lastAxis[l->lh] != (l->acode & 0x1)) ) + { + int lastAxis = dd->lastAxis[l->lh]; + //printf("\n"); + //if (0 == l->lh) + // printf("or[%d,%d] ", l->lh,lastAxis); + + for (int i=0; i < SENSORS_PER_OBJECT; i++) + { + //FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance + int index = dd->angleIndex[l->lh][axis]; + if (dd->oldAngles[i][axis][l->lh][dd->angleIndex[l->lh][axis]] != 0) + { + //if (0 == l->lh) + // printf("%d ", i); + + dd->hitCount[i][l->lh][axis]++; + } + else + { + dd->hitCount[i][l->lh][axis] *= 0.5; + } + } + //if (0 == l->lh) + // printf("\n"); + //int foo = l->acode & 0x1; + //printf("%d", foo); + + + //if (axis) + { + if (0 == l->lh && axis) // only once per full cycle... + { + static unsigned int counter = 1; + + counter++; + + // let's just do this occasionally for now... + if (counter % 4 == 0) + QuickPose(so); + } + // axis changed, time to increment the circular buffer index. + + + dd->angleIndex[l->lh][axis]++; + dd->angleIndex[l->lh][axis] = dd->angleIndex[l->lh][axis] % OLD_ANGLES_BUFF_LEN; + + // and clear out the data. + for (int i=0; i < SENSORS_PER_OBJECT; i++) + { + dd->oldAngles[i][axis][l->lh][dd->angleIndex[l->lh][axis]] = 0; + } + + } + dd->lastAxis[l->lh] = axis; + } + + //if (0 == l->lh) + // printf("(%d) ", l->sensor_id); + + //FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance + dd->oldAngles[l->sensor_id][axis][l->lh][dd->angleIndex[l->lh][axis]] = l->angle; + break; } case POSERDATA_FULL_SCENE: { TrackedObject *to; @@ -482,6 +662,7 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2]; to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + to->sensor[sensorCount].id=i; sensorCount++; } } @@ -509,6 +690,7 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2]; to->sensor[sensorCount].theta = fs->angles[i][lh][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) to->sensor[sensorCount].phi = fs->angles[i][lh][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + to->sensor[sensorCount].id=i; sensorCount++; } } diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 8b92860..22098d0 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -74,6 +74,11 @@ typedef struct FLT down[3]; // populated by the IMU for posing int something; //Stuff + +#define OLD_ANGLES_BUFF_LEN 3 + FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance + int angleIndex[NUM_LIGHTHOUSES][2]; // index into circular buffer ahead. separate index for each axis. + int lastAxis[NUM_LIGHTHOUSES]; } ToriData; @@ -526,7 +531,7 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, } - printf("\ni=%d\n", i); + printf(" i=%d ", i); return lastPoint; } @@ -845,12 +850,12 @@ void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagni static void WhereIsTheTrackedObjectAxisAngle(FLT *rotation, Point lhPoint) { - FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], -rotation[3]}; + FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], rotation[3]}; FLT objPoint[3] = {lhPoint.x, lhPoint.y, lhPoint.z}; rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); - printf("The tracked object is at location (%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); + printf("{%8.8f, %8.8f, %8.8f} ", objPoint[0], objPoint[1], objPoint[2]); } static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) @@ -873,7 +878,7 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini // in fact, it probably could probably be 1 without any issue. The main place where g is decremented // is in the block below when we've made a jump that results in a worse fitness than we're starting at. // In those cases, we don't take the jump, and instead lower the value of g and try again. - for (FLT g = 0.1; g > 0.000000001; g *= 0.99) + for (FLT g = 0.1; g > 0.000000001 || i > 10000; g *= 0.99) { i++; FLT point1[4]; @@ -947,7 +952,7 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini } - printf("\nRi=%d\n", i); + printf(" Ri=%d ", i); } static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint) { @@ -956,7 +961,7 @@ static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint) //rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); quatrotatevector(objPoint, rotation, objPoint); - printf("The tracked object is at location (%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); + printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); } @@ -1056,7 +1061,7 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in } - printf("\nRi=%d Fitness=%3f\n", i, lastMatchFitness); + printf("Ri=%3d Fitness=%3f ", i, lastMatchFitness); } @@ -1088,8 +1093,23 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) } -Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) +static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) { + //printf("Solving for Lighthouse\n"); + + //printf("obj->numSensors = %d;\n", obj->numSensors); + + //for (int i=0; i < obj->numSensors; i++) + //{ + // printf("obj->sensor[%d].normal.x = %f;\n", i, obj->sensor[i].normal.x); + // printf("obj->sensor[%d].normal.y = %f;\n", i, obj->sensor[i].normal.y); + // printf("obj->sensor[%d].normal.z = %f;\n", i, obj->sensor[i].normal.z); + // printf("obj->sensor[%d].point.x = %f;\n", i, obj->sensor[i].point.x); + // printf("obj->sensor[%d].point.y = %f;\n", i, obj->sensor[i].point.y); + // printf("obj->sensor[%d].point.z = %f;\n", i, obj->sensor[i].point.z); + // printf("obj->sensor[%d].phi = %f;\n", i, obj->sensor[i].phi); + // printf("obj->sensor[%d].theta = %f;\n\n", i, obj->sensor[i].theta); + //} PointsAndAngle pna[MAX_POINT_PAIRS]; volatile size_t sizeNeeded = sizeof(pna); @@ -1168,13 +1188,17 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount); - FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); - printf("(%4.4f, %4.4f, %4.4f)\n", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z); - printf("Distance is %f, Fitness is %f\n", distance, fitGd); + printf("(%4.4f, %4.4f, %4.4f) Dist: %8.8f Fit:%4f ", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance, fitGd); - FLT rot[4]; - SolveForRotation(rot, obj, refinedEstimateGd); + if (fitGd > 5) + { + FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); + printf("(%4.4f, %4.4f, %4.4f) Dist: %8.8f Fit:%4f ", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance, fitGd); + //printf("Distance is %f, Fitness is %f\n", distance, fitGd); + FLT rot[4]; + SolveForRotation(rot, obj, refinedEstimateGd); + } if (logFile) { updateHeader(logFile); @@ -1191,7 +1215,82 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) +static void QuickPose(SurviveObject *so) +{ + ToriData * td = so->PoserData; + + + //for (int i=0; i < so->nr_locations; i++) + //{ + // FLT x0=td->oldAngles[i][0][0][td->angleIndex[0][0]]; + // FLT y0=td->oldAngles[i][1][0][td->angleIndex[0][1]]; + // FLT x1=td->oldAngles[i][0][1][td->angleIndex[1][0]]; + // FLT y1=td->oldAngles[i][1][1][td->angleIndex[1][1]]; + // //printf("%2d: %8.8f, %8.8f %8.8f, %8.8f \n", + // // i, + // // x0, + // // y0, + // // x1, + // // y1 + // // ); + // printf("%2d: %8.8f, %8.8f \n", + // i, + // x0, + // y0 + // ); + //} + //printf("\n"); + + TrackedObject *to; + + to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor))); + { + int sensorCount = 0; + + for (int i = 0; i < so->nr_locations; i++) + { + int lh = 0; + int angleIndex0 = (td->angleIndex[lh][0] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN; + int angleIndex1 = (td->angleIndex[lh][1] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN; + if (td->oldAngles[i][0][lh][angleIndex0] != 0 && td->oldAngles[i][1][lh][angleIndex1] != 0) + { + FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; + FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; + + to->sensor[sensorCount].normal.x = norm[0]; + to->sensor[sensorCount].normal.y = norm[1]; + to->sensor[sensorCount].normal.z = norm[2]; + to->sensor[sensorCount].point.x = point[0]; + to->sensor[sensorCount].point.y = point[1]; + to->sensor[sensorCount].point.z = point[2]; + to->sensor[sensorCount].theta = td->oldAngles[i][0][lh][angleIndex0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = td->oldAngles[i][1][lh][angleIndex1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) + + //printf("%2d: %8.8f, %8.8f \n", + // i, + // to->sensor[sensorCount].theta, + // to->sensor[sensorCount].phi + // ); + + sensorCount++; + } + } + to->numSensors = sensorCount; + + if (sensorCount > 4) + { + SolveForLighthouse(to, 0); + printf("!\n"); + } + + + } + + + free(to); + +} @@ -1229,14 +1328,47 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) case POSERDATA_LIGHT: { PoserDataLight * l = (PoserDataLight*)poserData; + + if (l->lh >= NUM_LIGHTHOUSES || l->lh < 0) + { + // should never happen. Famous last words... + break; + } + int axis = l->acode & 0x1; //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode ); - if (0 == l->lh) + if ((td->lastAxis[l->lh] != (l->acode & 0x1)) ) { - if (l->acode & 0x1) + int foo = l->acode & 0x1; + //printf("%d", foo); + + + //if (axis) { - printf("%2d: %8f\n", l->sensor_id, l->angle); + if (0 == l->lh && axis) // only once per full cycle... + { + static unsigned int counter = 1; + + counter++; + + // let's just do this occasionally for now... + if (counter % 4 == 0) + QuickPose(so); + } + // axis changed, time to increment the circular buffer index. + td->angleIndex[l->lh][axis]++; + td->angleIndex[l->lh][axis] = td->angleIndex[l->lh][axis] % OLD_ANGLES_BUFF_LEN; + + // and clear out the data. + for (int i=0; i < SENSORS_PER_OBJECT; i++) + { + td->oldAngles[i][axis][l->lh][td->angleIndex[l->lh][axis]] = 0; + } + } + td->lastAxis[l->lh] = axis; } + + td->oldAngles[l->sensor_id][axis][l->lh][td->angleIndex[l->lh][axis]] = l->angle; break; } case POSERDATA_FULL_SCENE: @@ -1278,12 +1410,20 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) to->sensor[sensorCount].point.y = point[1]; to->sensor[sensorCount].point.z = point[2]; to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) - to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) + + //printf("%2d: %8.8f, %8.8f \n", + // i, + // to->sensor[sensorCount].theta, + // to->sensor[sensorCount].phi + // ); + sensorCount++; } } to->numSensors = sensorCount; + SolveForLighthouse(to, 0); } { diff --git a/src/survive_data.c b/src/survive_data.c index 3eb5890..3f16c7c 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -79,10 +79,40 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) } } + int allZero = 1; + for (int q=0; q< 32; q++) + if (lcd->sweep.sweep_len[q] != 0) + allZero=0; + //if (!allZero) + // printf("a[%d]l[%d] ", lcd->per_sweep.activeAcode & 5, lcd->per_sweep.activeLighthouse); for (int i = 0; i < SENSORS_PER_OBJECT; i++) { + { + static int counts[SENSORS_PER_OBJECT][2] = {0}; + + if (lcd->per_sweep.activeLighthouse == 0 && !allZero) + { + if (lcd->sweep.sweep_len[i] != 0) + { + //printf("%d ", i); + //counts[i][lcd->per_sweep.activeAcode & 1] ++; + } + else + { + counts[i][lcd->per_sweep.activeAcode & 1] =0; + } + + //if (counts[i][0] > 10 && counts[i][1] > 10) + //{ + //printf("%d(%d,%d), ", i, counts[i][0], counts[i][1]); + //} + } + } + + if (lcd->sweep.sweep_len[i] != 0) // if the sensor was hit, process it { + int offset_from = lcd->sweep.sweep_time[i] - lcd->per_sweep.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2; if (offset_from < 380000 && offset_from > 70000) @@ -94,6 +124,9 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) } } } + //if (!allZero) + // printf(" ..:..\n"); + } // clear out sweep data (could probably limit this to only after a "first" sync. // this is slightly more robust, so doing it here for now. @@ -108,6 +141,7 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) //static unsigned int recent_sync_count = -1; //static unsigned int activeSweepStartTime; + int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); // Process any sweep data we have handle_lightcap2_process_sweep_data(so); @@ -126,7 +160,6 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; - int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); if (!(acode >> 2 & 1)) // if the skip bit is not set { lcd->per_sweep.activeLighthouse = lcd->per_sweep.current_lh; @@ -148,8 +181,6 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; - int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); - if (!(acode >> 2 & 1)) // if the skip bit is not set { if (lcd->per_sweep.activeLighthouse != -1) -- cgit v1.2.3 From 9706cf9ee66134e5c37e8c860035d133b56dff66 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Wed, 29 Mar 2017 16:51:27 -0700 Subject: little bit o' spit 'n polish --- src/poser_turveytori.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 22098d0..76dd7fe 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1190,7 +1190,7 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) printf("(%4.4f, %4.4f, %4.4f) Dist: %8.8f Fit:%4f ", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance, fitGd); - if (fitGd > 5) + //if (fitGd > 5) { FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); printf("(%4.4f, %4.4f, %4.4f) Dist: %8.8f Fit:%4f ", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance, fitGd); @@ -1351,7 +1351,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) counter++; // let's just do this occasionally for now... - if (counter % 4 == 0) + if (counter % 1 == 0) QuickPose(so); } // axis changed, time to increment the circular buffer index. -- cgit v1.2.3 From a3837d79442e1845baf441b8f84b41da3e8d81bc Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 30 Mar 2017 11:07:39 -0700 Subject: Limit amount of time searching for best solution Sometimes the search algorithm would spend way too much time making a bunch of small improvements, when it would have been better to just stop and call it "good enough" --- src/poser_turveytori.c | 35 ++++++++++++++++++++++------------- 1 file changed, 22 insertions(+), 13 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 76dd7fe..47147b0 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -529,7 +529,16 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, } - + // from empiracle evidence, we're probably "good enough" at this point. + // So, even though we could still improve, we're likely to be improving + // very slowly, and we should just take what we've got and move on. + // This also seems to happen almost only when data is a little more "dirty" + // because the tracker is being rotated. + if (i > 120) + { + //printf("i got big"); + break; + } } printf(" i=%d ", i); @@ -950,7 +959,11 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini } - + if (i > 1000) + { + //printf("Ri got big"); + break; + } } printf(" Ri=%d ", i); } @@ -1188,18 +1201,14 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount); - printf("(%4.4f, %4.4f, %4.4f) Dist: %8.8f Fit:%4f ", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance, fitGd); + FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); + printf(" SensorCount(%d) LhPos:(%4.4f, %4.4f, %4.4f) Dist: %8.8f ", obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); + //printf("Distance is %f, Fitness is %f\n", distance, fitGd); - //if (fitGd > 5) - { - FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); - printf("(%4.4f, %4.4f, %4.4f) Dist: %8.8f Fit:%4f ", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance, fitGd); - //printf("Distance is %f, Fitness is %f\n", distance, fitGd); + FLT rot[4]; + SolveForRotation(rot, obj, refinedEstimateGd); - FLT rot[4]; - SolveForRotation(rot, obj, refinedEstimateGd); - } - if (logFile) + if (logFile) { updateHeader(logFile); fclose(logFile); @@ -1351,7 +1360,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) counter++; // let's just do this occasionally for now... - if (counter % 1 == 0) + if (counter % 2 == 0) QuickPose(so); } // axis changed, time to increment the circular buffer index. -- cgit v1.2.3 From 7ea900d3c0ce02bf5867e73dc2bbe3988207c477 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 30 Mar 2017 11:50:25 -0700 Subject: poser tweaks --- src/poser_turveytori.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 47147b0..5d25294 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -534,13 +534,13 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, // very slowly, and we should just take what we've got and move on. // This also seems to happen almost only when data is a little more "dirty" // because the tracker is being rotated. - if (i > 120) + if (i > 900) { //printf("i got big"); break; } } - printf(" i=%d ", i); + printf(" i=%3d ", i); return lastPoint; } -- cgit v1.2.3 From c783f758751b9a64ca09b41e6417222a5e6cbff2 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Thu, 30 Mar 2017 22:56:19 -0700 Subject: Checking Smallest Angle --- src/poser_turveytori.c | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 5d25294..c2d7410 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1129,6 +1129,8 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) Point avgNorm = { 0 }; + FLT smallestAngle = 20.0; + size_t pnaCount = 0; for (unsigned int i = 0; i < obj->numSensors; i++) { @@ -1143,6 +1145,11 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) //pna[pnaCount].angle = pythAngleBetweenSensors2(&obj->sensor[i], &obj->sensor[j]); pna[pnaCount].tanAngle = FLT_TAN(pna[pnaCount].angle); + if (pna[pnaCount].angle < smallestAngle) + { + smallestAngle = pna[pnaCount].angle; + } + double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta)); pna[pnaCount].rotation = GetRotationMatrixForTorus(pna[pnaCount].a, pna[pnaCount].b); @@ -1202,7 +1209,7 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount); FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); - printf(" SensorCount(%d) LhPos:(%4.4f, %4.4f, %4.4f) Dist: %8.8f ", obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); + printf(" sma(%d) SnsrCnt(%d) LhPos:(%4.4f, %4.4f, %4.4f) Dist: %8.8f ", smallestAngle, obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); //printf("Distance is %f, Fitness is %f\n", distance, fitGd); FLT rot[4]; -- cgit v1.2.3 From b562a8214508c74ed8094514816be012224abcd9 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 31 Mar 2017 10:23:08 -0700 Subject: hacks that make tori more usable --- src/poser_turveytori.c | 68 ++++++++++++++++++++++++++++++++++++++++++-------- 1 file changed, 57 insertions(+), 11 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index c2d7410..62cd12c 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -8,6 +8,11 @@ #include #include #include +#if defined(__FreeBSD__) || defined(__APPLE__) +#include +#else +#include //for alloca +#endif #define PointToFlts(x) ((FLT*)(x)) @@ -120,7 +125,8 @@ typedef struct FLT tanAngle; // tangent of angle Matrix3x3 rotation; Matrix3x3 invRotation; // inverse of rotation - + char ai; + char bi; } PointsAndAngle; @@ -378,21 +384,52 @@ FLT getPointFitnessForPna(Point pointIn, PointsAndAngle *pna) return dist; } -FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount) +FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount, int deubgPrint) { FLT fitness; FLT resultSum = 0; + FLT *fitnesses = alloca(sizeof(FLT) * pnaCount); + int i=0, j=0; + + FLT worstFitness = 0; for (size_t i = 0; i < pnaCount; i++) { fitness = getPointFitnessForPna(pointIn, &(pna[i])); - resultSum += SQUARED(fitness); + + if (worstFitness < fitness) + { + i = pna[i].ai; + j = pna[i].bi; + worstFitness = fitness; + } + + fitnesses[i] = fitness; + if (deubgPrint) + { + printf(" [%d, %d](%f)\n", pna[i].ai, pna[i].bi, fitness); + } } + for (size_t i = 0; i < pnaCount; i++) + { + // TODO: This is an UGLY HACK!!! It is NOT ROBUST and MUST BE BETTER + // Right now, we're just throwing away the single worst fitness value + // this works frequently, but we REALLY need to do a better job of determing + // exactly when we should throw away a bad value. I'm thinking that decision + // alone could be a master's thesis, so lots of work to be done. + // This is just a stupid general approach that helps in a lot of cases, + // but is NOT suitable for long term use. + //if (pna[i].bi != i && pna[i].bi != j && pna[i].ai != i && pna[i].ai != j) + if (fitnesses[i] != worstFitness) + resultSum += SQUARED(fitnesses[i]); + } return 1 / FLT_SQRT(resultSum); } +// TODO: Use a central point instead of separate "minus" points for each axis. This will reduce +// the number of fitness calls by 1/3. Point getGradient(Point pointIn, PointsAndAngle *pna, size_t pnaCount, FLT precision) { Point result; @@ -401,19 +438,19 @@ Point getGradient(Point pointIn, PointsAndAngle *pna, size_t pnaCount, FLT preci Point tmpXminus = pointIn; tmpXplus.x = pointIn.x + precision; tmpXminus.x = pointIn.x - precision; - result.x = getPointFitness(tmpXplus, pna, pnaCount) - getPointFitness(tmpXminus, pna, pnaCount); + result.x = getPointFitness(tmpXplus, pna, pnaCount, 0) - getPointFitness(tmpXminus, pna, pnaCount, 0); Point tmpYplus = pointIn; Point tmpYminus = pointIn; tmpYplus.y = pointIn.y + precision; tmpYminus.y = pointIn.y - precision; - result.y = getPointFitness(tmpYplus, pna, pnaCount) - getPointFitness(tmpYminus, pna, pnaCount); + result.y = getPointFitness(tmpYplus, pna, pnaCount, 0) - getPointFitness(tmpYminus, pna, pnaCount, 0); Point tmpZplus = pointIn; Point tmpZminus = pointIn; tmpZplus.z = pointIn.z + precision; tmpZminus.z = pointIn.z - precision; - result.z = getPointFitness(tmpZplus, pna, pnaCount) - getPointFitness(tmpZminus, pna, pnaCount); + result.z = getPointFitness(tmpZplus, pna, pnaCount, 0) - getPointFitness(tmpZminus, pna, pnaCount, 0); return result; } @@ -448,7 +485,7 @@ Point getAvgPoints(Point a, Point b) static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, FILE *logFile) { int i = 0; - FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount); + FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount, 0); Point lastPoint = initialEstimate; // The values below are somewhat magic, and definitely tunable @@ -505,7 +542,7 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, point4.y = point3.y + specialGradient.y; point4.z = point3.z + specialGradient.z; - FLT newMatchFitness = getPointFitness(point4, pna, pnaCount); + FLT newMatchFitness = getPointFitness(point4, pna, pnaCount, 0); if (newMatchFitness > lastMatchFitness) { @@ -534,7 +571,7 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, // very slowly, and we should just take what we've got and move on. // This also seems to happen almost only when data is a little more "dirty" // because the tracker is being rotated. - if (i > 900) + if (i > 120) { //printf("i got big"); break; @@ -1130,6 +1167,7 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) Point avgNorm = { 0 }; FLT smallestAngle = 20.0; + FLT largestAngle = 0; size_t pnaCount = 0; for (unsigned int i = 0; i < obj->numSensors; i++) @@ -1150,10 +1188,18 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) smallestAngle = pna[pnaCount].angle; } + if (pna[pnaCount].angle > largestAngle) + { + largestAngle = pna[pnaCount].angle; + } + double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta)); pna[pnaCount].rotation = GetRotationMatrixForTorus(pna[pnaCount].a, pna[pnaCount].b); pna[pnaCount].invRotation = inverseM33(pna[pnaCount].rotation); + pna[pnaCount].ai = i; + pna[pnaCount].bi = j; + pnaCount++; @@ -1206,10 +1252,10 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) } - FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount); + FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount, 0); FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); - printf(" sma(%d) SnsrCnt(%d) LhPos:(%4.4f, %4.4f, %4.4f) Dist: %8.8f ", smallestAngle, obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); + printf(" la(%f) SnsrCnt(%d) LhPos:(%4.4f, %4.4f, %4.4f) Dist: %8.8f ", largestAngle, obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); //printf("Distance is %f, Fitness is %f\n", distance, fitGd); FLT rot[4]; -- cgit v1.2.3 From 957945c223169073962b4987ee3bf73ab711d0a2 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 31 Mar 2017 10:41:18 -0700 Subject: formatting changes --- src/poser_turveytori.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 62cd12c..3bb8d59 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -901,7 +901,7 @@ static void WhereIsTheTrackedObjectAxisAngle(FLT *rotation, Point lhPoint) rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); - printf("{%8.8f, %8.8f, %8.8f} ", objPoint[0], objPoint[1], objPoint[2]); + printf("{% 08.8f, % 08.8f, % 08.8f} ", objPoint[0], objPoint[1], objPoint[2]); } static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) @@ -996,7 +996,7 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini } - if (i > 1000) + if (i > 998) { //printf("Ri got big"); break; @@ -1255,7 +1255,7 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount, 0); FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); - printf(" la(%f) SnsrCnt(%d) LhPos:(%4.4f, %4.4f, %4.4f) Dist: %8.8f ", largestAngle, obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); + printf(" la(% 04.4f) SnsrCnt(%2d) LhPos:(% 04.4f, % 04.4f, % 04.4f) Dist: % 08.8f ", largestAngle, (int)obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); //printf("Distance is %f, Fitness is %f\n", distance, fitGd); FLT rot[4]; -- cgit v1.2.3 From 4bcb16aae63e18085b73646a0c1dde94b10bb867 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 31 Mar 2017 14:29:52 -0700 Subject: Wonky World View --- src/poser_turveytori.c | 103 ++++++++++++++++++++++++++++++++++++++++--------- 1 file changed, 85 insertions(+), 18 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 3bb8d59..8b131b5 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -77,7 +77,6 @@ static const float DefaultPointsPerOuterDiameter = 60; typedef struct { FLT down[3]; // populated by the IMU for posing - int something; //Stuff #define OLD_ANGLES_BUFF_LEN 3 @@ -894,14 +893,15 @@ void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagni return; } -static void WhereIsTheTrackedObjectAxisAngle(FLT *rotation, Point lhPoint) +static void WhereIsTheTrackedObjectAxisAngle(FLT *posOut, FLT *rotation, Point lhPoint) { - FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], rotation[3]}; - FLT objPoint[3] = {lhPoint.x, lhPoint.y, lhPoint.z}; + posOut[0] = lhPoint.x; + posOut[1] = lhPoint.y; + posOut[2] = lhPoint.z; - rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); + rotatearoundaxis(posOut, posOut, rotation, rotation[3]); - printf("{% 08.8f, % 08.8f, % 08.8f} ", objPoint[0], objPoint[1], objPoint[2]); + printf("{% 04.4f, % 04.4f, % 04.4f} ", posOut[0], posOut[1], posOut[2]); } static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) @@ -1133,7 +1133,6 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) // Step 2, optimize the axis/ angle to match the data. RefineRotationEstimateAxisAngle(rotOut, lh, zAxis, obj); - WhereIsTheTrackedObjectAxisAngle(rotOut, lh); //// Step 2, optimize the quaternion to match the data. //RefineRotationEstimateQuaternion(rotOut, lh, quat1, obj); @@ -1143,7 +1142,7 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) } -static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) +static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, char doLogOutput, int lh, int setLhCalibration) { //printf("Solving for Lighthouse\n"); @@ -1258,10 +1257,55 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) printf(" la(% 04.4f) SnsrCnt(%2d) LhPos:(% 04.4f, % 04.4f, % 04.4f) Dist: % 08.8f ", largestAngle, (int)obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); //printf("Distance is %f, Fitness is %f\n", distance, fitGd); - FLT rot[4]; + FLT rot[4]; // this is axis/ angle rotation, not a quaternion! SolveForRotation(rot, obj, refinedEstimateGd); + FLT objPos[3]; + + WhereIsTheTrackedObjectAxisAngle(objPos, rot, refinedEstimateGd); + + FLT rotQuat[4]; + + quatfromaxisangle(rotQuat, rot, rot[3]); + + //{ + FLT tmpPos[3] = {refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z}; + + quatrotatevector(tmpPos, rotQuat, tmpPos); + //} + + if (setLhCalibration) + { + if (so->ctx->bsd[lh].PositionSet) + { + printf("Warning: resetting base station calibration data"); + } + + FLT invRot[4]; + quatgetreciprocal(invRot, rotQuat); + + so->ctx->bsd[lh].Pose.Pos[0] = refinedEstimateGd.x; + so->ctx->bsd[lh].Pose.Pos[1] = refinedEstimateGd.y; + so->ctx->bsd[lh].Pose.Pos[2] = refinedEstimateGd.z; + so->ctx->bsd[lh].Pose.Rot[0] = invRot[0]; + so->ctx->bsd[lh].Pose.Rot[1] = invRot[1]; + so->ctx->bsd[lh].Pose.Rot[2] = invRot[2]; + so->ctx->bsd[lh].Pose.Rot[3] = invRot[3]; + so->ctx->bsd[lh].PositionSet = 1; + } + + FLT wcPos[3]; // position in wold coordinates + + quatrotatevector(wcPos, so->ctx->bsd[lh].Pose.Rot, objPos); - if (logFile) + wcPos[0] -= so->ctx->bsd[lh].Pose.Pos[0]; + wcPos[1] -= so->ctx->bsd[lh].Pose.Pos[1]; + wcPos[2] -= so->ctx->bsd[lh].Pose.Pos[2]; + + printf(" <% 04.4f, % 04.4f, % 04.4f > ", wcPos[0], wcPos[1], wcPos[2]); + + //posOut = + + if (logFile) { updateHeader(logFile); fclose(logFile); @@ -1277,10 +1321,17 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) -static void QuickPose(SurviveObject *so) +static void QuickPose(SurviveObject *so, int lh) { + + ToriData * td = so->PoserData; + if (! so->ctx->bsd[lh].PositionSet) + { + // we don't know where we are! Augh!!! + return; + } //for (int i=0; i < so->nr_locations; i++) //{ @@ -1310,9 +1361,16 @@ static void QuickPose(SurviveObject *so) { int sensorCount = 0; + // TODO: remove, for debug purposes only! + FLT downQuat[4]; + FLT negZ[3] = { 0,0,-1 }; + quatfrom2vectors(downQuat, negZ, td->down); + //quatfrom2vectors(downQuat, td->down, negZ); + // end TODO + + for (int i = 0; i < so->nr_locations; i++) { - int lh = 0; int angleIndex0 = (td->angleIndex[lh][0] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN; int angleIndex1 = (td->angleIndex[lh][1] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN; if (td->oldAngles[i][0][lh][angleIndex0] != 0 && td->oldAngles[i][1][lh][angleIndex1] != 0) @@ -1320,6 +1378,10 @@ static void QuickPose(SurviveObject *so) FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; + // TODO: remove these two lines!!! + //quatrotatevector(norm, downQuat, norm); + //quatrotatevector(point, downQuat, point); + to->sensor[sensorCount].normal.x = norm[0]; to->sensor[sensorCount].normal.y = norm[1]; to->sensor[sensorCount].normal.z = norm[2]; @@ -1342,7 +1404,9 @@ static void QuickPose(SurviveObject *so) if (sensorCount > 4) { - SolveForLighthouse(to, 0); + FLT pos[3], quat[4]; + + SolveForLighthouse(pos, quat, to, so, 0, lh, 0); printf("!\n"); } @@ -1414,7 +1478,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) // let's just do this occasionally for now... if (counter % 2 == 0) - QuickPose(so); + QuickPose(so, 0); } // axis changed, time to increment the circular buffer index. td->angleIndex[l->lh][axis]++; @@ -1485,8 +1549,9 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) } to->numSensors = sensorCount; + FLT pos[3], quat[4]; - SolveForLighthouse(to, 0); + SolveForLighthouse(pos, quat, to, so, 0, 0, 1); } { int sensorCount = 0; @@ -1499,8 +1564,8 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; - quatrotatevector(norm, downQuat, norm); - quatrotatevector(point, downQuat, point); + //quatrotatevector(norm, downQuat, norm); + //quatrotatevector(point, downQuat, point); to->sensor[sensorCount].normal.x = norm[0]; to->sensor[sensorCount].normal.y = norm[1]; @@ -1516,7 +1581,9 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) to->numSensors = sensorCount; - SolveForLighthouse(to, 0); + FLT pos[3], quat[4]; + + SolveForLighthouse(pos, quat, to, so, 0, 1, 1); } free(to); -- cgit v1.2.3 From 198082258a62ab6313569e7950c0420baad6ddef Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 31 Mar 2017 15:19:00 -0700 Subject: Sane World View The z=0 plane is now initially aligned to the device's z=0 plane during calibration. (works really well for the tracker, which is oriented with Z in the proper direction when sitting on a horizontal surface) --- src/poser_turveytori.c | 30 ++++++++++++++++++------------ 1 file changed, 18 insertions(+), 12 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 8b131b5..d3dd2ff 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -895,9 +895,9 @@ void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagni static void WhereIsTheTrackedObjectAxisAngle(FLT *posOut, FLT *rotation, Point lhPoint) { - posOut[0] = lhPoint.x; - posOut[1] = lhPoint.y; - posOut[2] = lhPoint.z; + posOut[0] = -lhPoint.x; + posOut[1] = -lhPoint.y; + posOut[2] = -lhPoint.z; rotatearoundaxis(posOut, posOut, rotation, rotation[3]); @@ -1273,8 +1273,12 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob quatrotatevector(tmpPos, rotQuat, tmpPos); //} + //static int foo = 0; + + //if (0 == foo) if (setLhCalibration) - { + { + //foo = 1; if (so->ctx->bsd[lh].PositionSet) { printf("Warning: resetting base station calibration data"); @@ -1297,20 +1301,22 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob quatrotatevector(wcPos, so->ctx->bsd[lh].Pose.Rot, objPos); - wcPos[0] -= so->ctx->bsd[lh].Pose.Pos[0]; - wcPos[1] -= so->ctx->bsd[lh].Pose.Pos[1]; - wcPos[2] -= so->ctx->bsd[lh].Pose.Pos[2]; + wcPos[0] += so->ctx->bsd[lh].Pose.Pos[0]; + wcPos[1] += so->ctx->bsd[lh].Pose.Pos[1]; + wcPos[2] += so->ctx->bsd[lh].Pose.Pos[2]; - printf(" <% 04.4f, % 04.4f, % 04.4f > ", wcPos[0], wcPos[1], wcPos[2]); + so->OutPose.Pos[0] = wcPos[0]; + so->OutPose.Pos[1] = wcPos[1]; + so->OutPose.Pos[2] = wcPos[2]; - //posOut = + printf(" <% 04.4f, % 04.4f, % 04.4f > ", wcPos[0], wcPos[1], wcPos[2]); if (logFile) { updateHeader(logFile); fclose(logFile); } - //fgetc(stdin); + return refinedEstimateGd; } @@ -1526,8 +1532,8 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; - quatrotatevector(norm, downQuat, norm); - quatrotatevector(point, downQuat, point); + //quatrotatevector(norm, downQuat, norm); + //quatrotatevector(point, downQuat, point); to->sensor[sensorCount].normal.x = norm[0]; to->sensor[sensorCount].normal.y = norm[1]; -- cgit v1.2.3 From eb5fae1c2aa6007ec84fdf0c5405308c7ea274ca Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 31 Mar 2017 15:30:35 -0700 Subject: Cleanup --- src/poser_turveytori.c | 58 ++++++++++++++++++-------------------------------- 1 file changed, 21 insertions(+), 37 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index d3dd2ff..6997aa7 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1367,12 +1367,12 @@ static void QuickPose(SurviveObject *so, int lh) { int sensorCount = 0; - // TODO: remove, for debug purposes only! - FLT downQuat[4]; - FLT negZ[3] = { 0,0,-1 }; - quatfrom2vectors(downQuat, negZ, td->down); + //// TODO: remove, for debug purposes only! + //FLT downQuat[4]; + //FLT negZ[3] = { 0,0,-1 }; + ////quatfrom2vectors(downQuat, negZ, td->down); //quatfrom2vectors(downQuat, td->down, negZ); - // end TODO + //// end TODO for (int i = 0; i < so->nr_locations; i++) @@ -1397,11 +1397,6 @@ static void QuickPose(SurviveObject *so, int lh) to->sensor[sensorCount].theta = td->oldAngles[i][0][lh][angleIndex0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) to->sensor[sensorCount].phi = td->oldAngles[i][1][lh][angleIndex1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) - //printf("%2d: %8.8f, %8.8f \n", - // i, - // to->sensor[sensorCount].theta, - // to->sensor[sensorCount].phi - // ); sensorCount++; } @@ -1470,33 +1465,28 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode ); if ((td->lastAxis[l->lh] != (l->acode & 0x1)) ) { - int foo = l->acode & 0x1; - //printf("%d", foo); - //if (axis) + if (0 == l->lh && axis) // only once per full cycle... { - if (0 == l->lh && axis) // only once per full cycle... - { - static unsigned int counter = 1; + static unsigned int counter = 1; - counter++; + counter++; - // let's just do this occasionally for now... - if (counter % 2 == 0) - QuickPose(so, 0); - } - // axis changed, time to increment the circular buffer index. - td->angleIndex[l->lh][axis]++; - td->angleIndex[l->lh][axis] = td->angleIndex[l->lh][axis] % OLD_ANGLES_BUFF_LEN; - - // and clear out the data. - for (int i=0; i < SENSORS_PER_OBJECT; i++) - { - td->oldAngles[i][axis][l->lh][td->angleIndex[l->lh][axis]] = 0; - } + // let's just do this occasionally for now... + if (counter % 2 == 0) + QuickPose(so, 0); + } + // axis changed, time to increment the circular buffer index. + td->angleIndex[l->lh][axis]++; + td->angleIndex[l->lh][axis] = td->angleIndex[l->lh][axis] % OLD_ANGLES_BUFF_LEN; + // and clear out the data. + for (int i=0; i < SENSORS_PER_OBJECT; i++) + { + td->oldAngles[i][axis][l->lh][td->angleIndex[l->lh][axis]] = 0; } + td->lastAxis[l->lh] = axis; } @@ -1517,7 +1507,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) // let's get the quaternion that represents this rotation. FLT downQuat[4]; - FLT negZ[3] = { 0,0,-1 }; + FLT negZ[3] = { 0,0,1 }; //quatfrom2vectors(downQuat, negZ, td->down); quatfrom2vectors(downQuat, td->down, negZ); @@ -1544,12 +1534,6 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) - //printf("%2d: %8.8f, %8.8f \n", - // i, - // to->sensor[sensorCount].theta, - // to->sensor[sensorCount].phi - // ); - sensorCount++; } } -- cgit v1.2.3 From 66eb9b0508c6a5b1af5d1bc8fb5cac74573f8b20 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 31 Mar 2017 15:36:36 -0700 Subject: Added rotation to object pose Haven't tested it yet, low confidence it's right, but the code is on the right track. --- src/poser_turveytori.c | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 6997aa7..c251040 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1301,6 +1301,9 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob quatrotatevector(wcPos, so->ctx->bsd[lh].Pose.Rot, objPos); + FLT newOrientation[4]; + quatrotateabout(newOrientation, rotQuat, so->ctx->bsd[lh].Pose.Rot ); + wcPos[0] += so->ctx->bsd[lh].Pose.Pos[0]; wcPos[1] += so->ctx->bsd[lh].Pose.Pos[1]; wcPos[2] += so->ctx->bsd[lh].Pose.Pos[2]; @@ -1309,6 +1312,11 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob so->OutPose.Pos[1] = wcPos[1]; so->OutPose.Pos[2] = wcPos[2]; + so->OutPose.Rot[0] = newOrientation[0]; + so->OutPose.Rot[1] = newOrientation[1]; + so->OutPose.Rot[2] = newOrientation[2]; + so->OutPose.Rot[3] = newOrientation[3]; + printf(" <% 04.4f, % 04.4f, % 04.4f > ", wcPos[0], wcPos[1], wcPos[2]); if (logFile) -- cgit v1.2.3 From 0e69fac4d8565926ac3cff6084c95f204d8e173f Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Sat, 1 Apr 2017 23:37:03 -0700 Subject: Fix bug that prevented HMD from being added --- src/survive_vive.c | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/survive_vive.c b/src/survive_vive.c index 757fa11..5224728 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -68,14 +68,14 @@ const char * devnames[] = { #define USB_DEV_HMD 0 -#define USB_DEV_LIGHTHOUSE 1 +#define USB_DEV_HMD_IMU_LH 1 #define USB_DEV_WATCHMAN1 2 #define USB_DEV_WATCHMAN2 3 #define USB_DEV_TRACKER0 4 #define USB_DEV_W_WATCHMAN1 5 // Wired Watchman attached via USB #ifdef HIDAPI -#define USB_DEV_LIGHTHOUSEB 6 +#define USB_DEV_HMD_IMU_LHB 6 #define USB_DEV_TRACKER0_LIGHTCAP 7 #define USB_DEV_W_WATCHMAN1_LIGHTCAP 8 #define MAX_USB_DEVS 9 @@ -84,7 +84,7 @@ const char * devnames[] = { #endif #define USB_IF_HMD 0 -#define USB_IF_LIGHTHOUSE 1 +#define USB_IF_HMD_IMU_LH 1 #define USB_IF_WATCHMAN1 2 #define USB_IF_WATCHMAN2 3 #define USB_IF_TRACKER0 4 @@ -471,14 +471,14 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * #endif if( sv->udev[USB_DEV_HMD] && AttachInterface( sv, hmd, USB_IF_HMD, sv->udev[USB_DEV_HMD], 0x81, survive_data_cb, "Mainboard" ) ) { return -6; } - if( sv->udev[USB_DEV_LIGHTHOUSE] && AttachInterface( sv, hmd, USB_IF_LIGHTHOUSE, sv->udev[USB_DEV_LIGHTHOUSE], 0x81, survive_data_cb, "Lighthouse" ) ) { return -7; } + if( sv->udev[USB_DEV_HMD_IMU_LH] && AttachInterface( sv, hmd, USB_IF_HMD_IMU_LH, sv->udev[USB_DEV_HMD_IMU_LH], 0x81, survive_data_cb, "Lighthouse" ) ) { return -7; } if( sv->udev[USB_DEV_WATCHMAN1] && AttachInterface( sv, wm0, USB_IF_WATCHMAN1, sv->udev[USB_DEV_WATCHMAN1], 0x81, survive_data_cb, "Watchman 1" ) ) { return -8; } if( sv->udev[USB_DEV_WATCHMAN2] && AttachInterface( sv, wm1, USB_IF_WATCHMAN2, sv->udev[USB_DEV_WATCHMAN2], 0x81, survive_data_cb, "Watchman 2")) { return -9; } if( sv->udev[USB_DEV_TRACKER0] && AttachInterface( sv, tr0, USB_IF_TRACKER0, sv->udev[USB_DEV_TRACKER0], 0x81, survive_data_cb, "Tracker 1")) { return -10; } if( sv->udev[USB_DEV_W_WATCHMAN1] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1, sv->udev[USB_DEV_W_WATCHMAN1], 0x81, survive_data_cb, "Wired Watchman 1")) { return -11; } #ifdef HIDAPI //Tricky: use other interface for actual lightcap. XXX THIS IS NOT YET RIGHT!!! - if( sv->udev[USB_DEV_LIGHTHOUSEB] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_LIGHTHOUSEB], 0x82, survive_data_cb, "Lightcap")) { return -12; } + if( sv->udev[USB_DEV_HMD_IMU_LHB] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_HMD_IMU_LHB], 0x82, survive_data_cb, "Lightcap")) { return -12; } // This is a HACK! But it works. Need to investigate further sv->uiface[USB_DEV_TRACKER0_LIGHTCAP].actual_len = 64; @@ -517,14 +517,14 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo if( r != sizeof( vive_magic_power_on ) ) return 5; } - if (sv->udev[USB_DEV_LIGHTHOUSE]) + if (sv->udev[USB_DEV_HMD_IMU_LH]) { static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; - r = update_feature_report( sv->udev[USB_DEV_LIGHTHOUSE], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); + r = update_feature_report( sv->udev[USB_DEV_HMD_IMU_LH], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates) - r = update_feature_report( sv->udev[USB_DEV_LIGHTHOUSE], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); + r = update_feature_report( sv->udev[USB_DEV_HMD_IMU_LH], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; } @@ -1103,7 +1103,7 @@ void survive_data_cb( SurviveUSBInterface * si ) headset->ison = 1; break; } - case USB_IF_LIGHTHOUSE: + case USB_IF_HMD_IMU_LH: case USB_IF_W_WATCHMAN1: case USB_IF_TRACKER0: { @@ -1395,7 +1395,7 @@ int DriverRegHTCVive( SurviveContext * ctx ) } //Next, pull out the config stuff. - if( sv->udev[USB_DEV_HMD] && LoadConfig( sv, hmd, 1, 0, 0 )) { SV_INFO( "HMD config issue." ); } + if( sv->udev[USB_DEV_HMD_IMU_LH] && LoadConfig( sv, hmd, 1, 0, 0 )) { SV_INFO( "HMD config issue." ); } if( sv->udev[USB_DEV_WATCHMAN1] && LoadConfig( sv, wm0, 2, 0, 1 )) { SV_INFO( "Watchman 0 config issue." ); } if( sv->udev[USB_DEV_WATCHMAN2] && LoadConfig( sv, wm1, 3, 0, 1 )) { SV_INFO( "Watchman 1 config issue." ); } if( sv->udev[USB_DEV_TRACKER0] && LoadConfig( sv, tr0, 4, 0, 0 )) { SV_INFO( "Tracker 0 config issue." ); } @@ -1450,7 +1450,7 @@ int DriverRegHTCVive( SurviveContext * ctx ) */ //Add the drivers. - if( sv->udev[USB_DEV_HMD] ) { survive_add_object( ctx, hmd ); } + if( sv->udev[USB_DEV_HMD_IMU_LH] ) { survive_add_object( ctx, hmd ); } if( sv->udev[USB_DEV_WATCHMAN1] ) { survive_add_object( ctx, wm0 ); } if( sv->udev[USB_DEV_WATCHMAN2] ) { survive_add_object( ctx, wm1 ); } if( sv->udev[USB_DEV_TRACKER0] ) { survive_add_object( ctx, tr0 ); } -- cgit v1.2.3 From ed6f9dc1a9247581f13ecc6943823e04f0dcd4ca Mon Sep 17 00:00:00 2001 From: cnlohr Date: Tue, 4 Apr 2017 23:18:52 -0400 Subject: Add diagrams of how it all fits together. --- useful_files/FunctionalSystem.dia | Bin 0 -> 5149 bytes useful_files/FunctionalSystem.png | Bin 0 -> 145206 bytes 2 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 useful_files/FunctionalSystem.dia create mode 100644 useful_files/FunctionalSystem.png diff --git a/useful_files/FunctionalSystem.dia b/useful_files/FunctionalSystem.dia new file mode 100644 index 0000000..d231ef5 Binary files /dev/null and b/useful_files/FunctionalSystem.dia differ diff --git a/useful_files/FunctionalSystem.png b/useful_files/FunctionalSystem.png new file mode 100644 index 0000000..030f464 Binary files /dev/null and b/useful_files/FunctionalSystem.png differ -- cgit v1.2.3 From d831f9ab967e4cf9f425e68799a0491f711a8a54 Mon Sep 17 00:00:00 2001 From: CNLohr Date: Tue, 4 Apr 2017 23:59:02 -0400 Subject: Update README.md --- README.md | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/README.md b/README.md index db1d6ce..7ea932e 100644 --- a/README.md +++ b/README.md @@ -70,6 +70,24 @@ It is written in some fairly stout "layers" which are basically just function ca | survive_data.c | Raw HID messages are processed into logical "light" "analog" and "imu" messages. | Mostly done, Missing light data from controllers, and lighthouse data. | | survive_process.c | Process the high-level data into solutions for | Not yet started. Will be done by ultramn | + + + + + +
DescriptionDiagram
+LibSurvive use a fairly straightforward architecture. + +There are device drivers, such as survive_vive.c which connect to physical devices, via libUSB, hidapi or another method and collect raw IMU and lightcap data. Lightcap data is specically a sensor ID, light pulse length (in ticks) and the time of the light pulse (in ticks). The driver also must provide locations of the sensors to populate the SurviveObject structures of whatever sensors that driver is responsible for. + +Once this data is collected, the light pulses are disambiguated (see survive_data.c) into OOTX sync pulses (id -1, -2 depending on lighthouse) as well as sweep pulses which provide the time of a sweep pulse passing the sensor. This is passed off to "lightproc." The default behavior for lightproc can be found in survive.c. The user may override this, however, if they have interest in the raw pulse information for whatever reason. The default behavior for lightproc determines the time delta between the sync pulse and the sweep pulse time and derives angle. The derivation for the angle is simply calculated by the time difference between the sync pulse and the sweep pulse. It then calls "angleproc" (not implemented yet: Using OOTX data from lighthouses to correct and tweak angles) + +Angleproc may also be overridden by the user for similar purposes to for "angleproc" which passes its information off to a calibrator (if running) as well as to whatever posers are enabled. The posers will take this data and determine position from it. + +
+ + + I may or may not read data from the Vive regarding configuration. If I do, it would be added to the survive_usb.c ## Intel Integrated Graphics -- cgit v1.2.3 From 13e06951c7a9aa6ad10002ce4f80bd810dd562e3 Mon Sep 17 00:00:00 2001 From: CNLohr Date: Wed, 5 Apr 2017 00:11:28 -0400 Subject: Update README.md --- README.md | 18 ++++++------------ 1 file changed, 6 insertions(+), 12 deletions(-) diff --git a/README.md b/README.md index 7ea932e..966370e 100644 --- a/README.md +++ b/README.md @@ -60,20 +60,16 @@ If I ever get to video output... OpenGL. ## Architecture -There is an internal representation and an external representation. These lines may get blurred. Internal representation lives in .h files in the ```src/``` folder. External lives in ```include/``` folder. - -It is written in some fairly stout "layers" which are basically just function calls: - -| Layer | Description | Status | -| ------- | ------------- | -------- | -| survive_usb.c | Data is taken in at "survive_usb.c" from libusb. | Done | -| survive_data.c | Raw HID messages are processed into logical "light" "analog" and "imu" messages. | Mostly done, Missing light data from controllers, and lighthouse data. | -| survive_process.c | Process the high-level data into solutions for | Not yet started. Will be done by ultramn |
DescriptionDiagram
-LibSurvive use a fairly straightforward architecture. + +### Layout + +In the src/ folder you'll find most of the internal code that is part of libsurvive. The redist/ folder contains code that libsurvive uses that was copied from other projects. Libsurvive links to other libraries, but very few. You'll find that most of the functionality lies within libsurvive or in the redist folder. For the user-facing headers you can find them in the include/ folder. + +### Logical Data Flow There are device drivers, such as survive_vive.c which connect to physical devices, via libUSB, hidapi or another method and collect raw IMU and lightcap data. Lightcap data is specically a sensor ID, light pulse length (in ticks) and the time of the light pulse (in ticks). The driver also must provide locations of the sensors to populate the SurviveObject structures of whatever sensors that driver is responsible for. @@ -88,8 +84,6 @@ Angleproc may also be overridden by the user for similar purposes to for "anglep -I may or may not read data from the Vive regarding configuration. If I do, it would be added to the survive_usb.c - ## Intel Integrated Graphics The limiting factor for Vive viability on a given computer is the maximum available pixel clock frequency, and frequency limitations of the HDMI port, and HDMI and DisplayPort video cables. DisplayPort can support higher frequencies than HDMI, on Ivy Bridge HD4000 graphics. In fact, the Vive works with HD4000 graphics using DisplayPort, with native EDID resolution (2160x1200@90Hz). -- cgit v1.2.3 From 58af165b297f1d5946b5b580d1ff5d65f26636ee Mon Sep 17 00:00:00 2001 From: CNLohr Date: Wed, 5 Apr 2017 00:20:35 -0400 Subject: Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 966370e..db659bd 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,7 @@ ![Logo](https://cloud.githubusercontent.com/assets/2748168/24084003/9095c98a-0cb8-11e7-88a3-575f9f4c7bb4.png) -An Open-Source tool for working with lighthouse-based trakcing data, including support for the HTC Vive, which is still in the experimental phase. +An Open-Source tool for working with lighthouse-based tracking data, including support for the HTC Vive, which is still in the experimental phase. Most of the development is discussed on Discord. Join the chat and discussion here: https://discordapp.com/invite/7QbCAGS -- cgit v1.2.3 From 443811f15c00df3df9cc14bc859e4cce146cd55f Mon Sep 17 00:00:00 2001 From: mwturvey Date: Wed, 5 Apr 2017 11:26:09 -0700 Subject: Fix USB_DEV_HMD_IMU_LH --- src/survive_vive.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/survive_vive.c b/src/survive_vive.c index 5224728..1f0b5b9 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -485,7 +485,7 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * if( sv->udev[USB_DEV_TRACKER0_LIGHTCAP] && AttachInterface( sv, tr0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0x82, survive_data_cb, "Tracker 1 Lightcap")) { return -13; } if( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; } #else - if( sv->udev[USB_DEV_LIGHTHOUSE] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_LIGHTHOUSE], 0x82, survive_data_cb, "Lightcap")) { return -12; } + if( sv->udev[USB_DEV_HMD_IMU_LH] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_HMD_IMU_LH], 0x82, survive_data_cb, "Lightcap")) { return -12; } if( sv->udev[USB_DEV_TRACKER0] && AttachInterface( sv, ww0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0], 0x82, survive_data_cb, "Tracker 0 Lightcap")) { return -13; } if( sv->udev[USB_DEV_W_WATCHMAN1] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; } #endif -- cgit v1.2.3 From 299d8af12c51330c54ea20b14dc25732e897b01f Mon Sep 17 00:00:00 2001 From: CNLohr Date: Thu, 6 Apr 2017 22:25:14 -0400 Subject: Update README.md --- README.md | 40 ++++++++++++++++++++++++++++++---------- 1 file changed, 30 insertions(+), 10 deletions(-) diff --git a/README.md b/README.md index db659bd..4395d0e 100644 --- a/README.md +++ b/README.md @@ -44,23 +44,18 @@ I say "high-performance" really this project is based tightly off of OSVR-Vive-L 4. Avoid extra layers where convenient. 5. (long shot) Make the vive viable for use with Intel Integrated Graphics systems. [It works with HD4000 using DisplayPort. See "Intel Integrated Graphics" section below.] -Will ~~I~~ we succeed? Probably not. - -Definitely going to try! +Will ~~I~~ we succeed? Probably not. ~~Definitely going to try!~~ Though it's looking like we might. ## External dependencies -* libUSB (hidapi ???) +* libUSB (Linux) or hidapi (Win, OSX; included in redist) * pthread -* libX11 (where applicable) -* zlib (may use puff.c if needed) - -If I ever get to video output... OpenGL. +* libX11 (Linux) or Native (win32) or OpenGL (OSX) +* zlib (Linux) or puff.c (win32, included in redist) ## Architecture -
DescriptionDiagram
@@ -82,7 +77,31 @@ Angleproc may also be overridden by the user for similar purposes to for "anglep
- +## Lists of components + +Component Type | Component | Description | Authors +--- | --- | --- | --- +Poser | src/poser_charlesslow.c | A very slow, but exhaustive poser system. Calibration only. | @cnlohr +Poser | src/poser_daveortho.c | A very fast system using orthograpic view and affine transformations. Calibration only (for now) | @ultramn +Poser | src/poser_dummy.c | Template for posers | @cnlohr +Poser | src/poser_octavioradii.c | ... | @mwturvey and Octavio +Poser | src/poser_turveytori.c | ... | @mwturvey +Disambiguator | src/survive_data.c (currently #ifdefed out) | The old disambiguator - very fast, but slightly buggy. | @cnlohr +Disambiguator | src/survive_data.c (current disambiguator) | More complicated but much more robust disambiguator | @mwturvey +Dismabiguator | superceded disambiguator | A more sophisticated disambiguator, development abandoned. Removed from tree. | @jpicht +Driver | src/survive_vive.c | Driver for HTC Vive HMD, Watchmen (wired+wireless) and Tracker | @cnlohr +OOTX Decoder | src/ootx_decoder.c | The system that takes the pulse-codes from the sync pulses from the lighthouses and get [OOTX Data](https://github.com/nairol/LighthouseRedox/blob/master/docs/Light%20Emissions.md) | @axlecrusher + +## Component Pluggability Matrix + +Component Type | Pluggability method +--- | --- +Driver | Dynamically loadable runtime, can co-exist with other drivers. +Poser | Selectable by configuration at runtime +Disambiguator | Selectable by #define +OOTX Decoder | Not Pluggable + + ## Intel Integrated Graphics @@ -95,3 +114,4 @@ To support the Vive on HDMI, you either need a newer version of HDMI, or you nee ## Addendum and notes Thanks to Mr. Fault for our logo! +Special thanks to @nairol for an extreme amount of detail in reverse engineering the existing HTC Vive system on his https://github.com/nairol/LighthouseRedox project. -- cgit v1.2.3 From 51baee4bc2c2b3990b878f75baa0abc83027c36c Mon Sep 17 00:00:00 2001 From: CNLohr Date: Thu, 6 Apr 2017 22:34:59 -0400 Subject: Update README.md --- README.md | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/README.md b/README.md index 4395d0e..a6d3b8a 100644 --- a/README.md +++ b/README.md @@ -81,16 +81,16 @@ Angleproc may also be overridden by the user for similar purposes to for "anglep Component Type | Component | Description | Authors --- | --- | --- | --- -Poser | src/poser_charlesslow.c | A very slow, but exhaustive poser system. Calibration only. | @cnlohr -Poser | src/poser_daveortho.c | A very fast system using orthograpic view and affine transformations. Calibration only (for now) | @ultramn -Poser | src/poser_dummy.c | Template for posers | @cnlohr -Poser | src/poser_octavioradii.c | ... | @mwturvey and Octavio -Poser | src/poser_turveytori.c | ... | @mwturvey -Disambiguator | src/survive_data.c (currently #ifdefed out) | The old disambiguator - very fast, but slightly buggy. | @cnlohr -Disambiguator | src/survive_data.c (current disambiguator) | More complicated but much more robust disambiguator | @mwturvey -Dismabiguator | superceded disambiguator | A more sophisticated disambiguator, development abandoned. Removed from tree. | @jpicht -Driver | src/survive_vive.c | Driver for HTC Vive HMD, Watchmen (wired+wireless) and Tracker | @cnlohr -OOTX Decoder | src/ootx_decoder.c | The system that takes the pulse-codes from the sync pulses from the lighthouses and get [OOTX Data](https://github.com/nairol/LighthouseRedox/blob/master/docs/Light%20Emissions.md) | @axlecrusher +Poser | [poser_charlesslow.c](src/poser_charlesslow.c) | A very slow, but exhaustive poser system. Calibration only. | [@cnlohr](https://github.com/cnlohr) and [@mwturvey](https://github.com/mwturvey) +Poser | [poser_daveortho.c](src/poser_daveortho.c) | A very fast system using orthograpic view and affine transformations. Calibration only (for now) | [@ultramn](https://github.com/ultramn) +Poser | [poser_dummy.c](src/poser_dummy.c) | Template for posers | [@cnlohr](https://github.com/cnlohr) +Poser | [poser_octavioradii.c](src/poser_octavioradii.c) | ... | [@mwturvey](https://github.com/mwturvey) and Octavio +Poser | [poser_turveytori.c](src/poser_turveytori.c) | ... | [@mwturvey](https://github.com/mwturvey) +Disambiguator | [survive_data.c](src/survive_data.c) (currently #ifdefed out) | The old disambiguator - very fast, but slightly buggy. | [@cnlohr](https://github.com/cnlohr) +Disambiguator | [survive_data.c](src/survive_data.c) (current disambiguator) | More complicated but much more robust disambiguator | [@mwturvey](https://github.com/mwturvey) +Dismabiguator | superceded disambiguator | A more sophisticated disambiguator, development abandoned. Removed from tree. | [@jpicht](https://github.com/jpicht) +Driver | [survive_vive.c](src/survive_vive.c) | Driver for HTC Vive HMD, Watchmen (wired+wireless) and Tracker | [@cnlohr](https://github.com/cnlohr) +OOTX Decoder | [ootx_decoder.c](src/ootx_decoder.c) | The system that takes the pulse-codes from the sync pulses from the lighthouses and get [OOTX Data](https://github.com/nairol/LighthouseRedox/blob/master/docs/Light%20Emissions.md) | [@axlecrusher](https://github.com/axlecrusher) ## Component Pluggability Matrix -- cgit v1.2.3 From 4c3c9588fc198759e368d93b42a1b150901e853c Mon Sep 17 00:00:00 2001 From: CNLohr Date: Thu, 6 Apr 2017 22:44:00 -0400 Subject: Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index a6d3b8a..8898395 100644 --- a/README.md +++ b/README.md @@ -84,7 +84,7 @@ Component Type | Component | Description | Authors Poser | [poser_charlesslow.c](src/poser_charlesslow.c) | A very slow, but exhaustive poser system. Calibration only. | [@cnlohr](https://github.com/cnlohr) and [@mwturvey](https://github.com/mwturvey) Poser | [poser_daveortho.c](src/poser_daveortho.c) | A very fast system using orthograpic view and affine transformations. Calibration only (for now) | [@ultramn](https://github.com/ultramn) Poser | [poser_dummy.c](src/poser_dummy.c) | Template for posers | [@cnlohr](https://github.com/cnlohr) -Poser | [poser_octavioradii.c](src/poser_octavioradii.c) | ... | [@mwturvey](https://github.com/mwturvey) and Octavio +Poser | [poser_octavioradii.c](src/poser_octavioradii.c) | ... | [@mwturvey](https://github.com/mwturvey) and [@octavio2895](https://github.com/octavio2895) Poser | [poser_turveytori.c](src/poser_turveytori.c) | ... | [@mwturvey](https://github.com/mwturvey) Disambiguator | [survive_data.c](src/survive_data.c) (currently #ifdefed out) | The old disambiguator - very fast, but slightly buggy. | [@cnlohr](https://github.com/cnlohr) Disambiguator | [survive_data.c](src/survive_data.c) (current disambiguator) | More complicated but much more robust disambiguator | [@mwturvey](https://github.com/mwturvey) -- cgit v1.2.3 From b72ee8cd0acfb78ca2a2a24c40504d764c137602 Mon Sep 17 00:00:00 2001 From: CNLohr Date: Thu, 6 Apr 2017 22:47:19 -0400 Subject: Update README.md --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 8898395..e3b589e 100644 --- a/README.md +++ b/README.md @@ -81,7 +81,7 @@ Angleproc may also be overridden by the user for similar purposes to for "anglep Component Type | Component | Description | Authors --- | --- | --- | --- -Poser | [poser_charlesslow.c](src/poser_charlesslow.c) | A very slow, but exhaustive poser system. Calibration only. | [@cnlohr](https://github.com/cnlohr) and [@mwturvey](https://github.com/mwturvey) +Poser | [poser_charlesslow.c](src/poser_charlesslow.c) | A very slow, but exhaustive poser system. Calibration only. | [@cnlohr](https://github.com/cnlohr) Poser | [poser_daveortho.c](src/poser_daveortho.c) | A very fast system using orthograpic view and affine transformations. Calibration only (for now) | [@ultramn](https://github.com/ultramn) Poser | [poser_dummy.c](src/poser_dummy.c) | Template for posers | [@cnlohr](https://github.com/cnlohr) Poser | [poser_octavioradii.c](src/poser_octavioradii.c) | ... | [@mwturvey](https://github.com/mwturvey) and [@octavio2895](https://github.com/octavio2895) @@ -89,7 +89,7 @@ Poser | [poser_turveytori.c](src/poser_turveytori.c) | ... | [@mwturvey](https:/ Disambiguator | [survive_data.c](src/survive_data.c) (currently #ifdefed out) | The old disambiguator - very fast, but slightly buggy. | [@cnlohr](https://github.com/cnlohr) Disambiguator | [survive_data.c](src/survive_data.c) (current disambiguator) | More complicated but much more robust disambiguator | [@mwturvey](https://github.com/mwturvey) Dismabiguator | superceded disambiguator | A more sophisticated disambiguator, development abandoned. Removed from tree. | [@jpicht](https://github.com/jpicht) -Driver | [survive_vive.c](src/survive_vive.c) | Driver for HTC Vive HMD, Watchmen (wired+wireless) and Tracker | [@cnlohr](https://github.com/cnlohr) +Driver | [survive_vive.c](src/survive_vive.c) | Driver for HTC Vive HMD, Watchmen (wired+wireless) and Tracker | [@cnlohr](https://github.com/cnlohr) and [@mwturvey](https://github.com/mwturvey) OOTX Decoder | [ootx_decoder.c](src/ootx_decoder.c) | The system that takes the pulse-codes from the sync pulses from the lighthouses and get [OOTX Data](https://github.com/nairol/LighthouseRedox/blob/master/docs/Light%20Emissions.md) | [@axlecrusher](https://github.com/axlecrusher) ## Component Pluggability Matrix -- cgit v1.2.3 From af0427bea12da9377eca4cc6a768762cf5a311c4 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Fri, 7 Apr 2017 00:52:11 -0400 Subject: Make output more visually clear (expanding window of possible area for angle sources. --- calibrate.c | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/calibrate.c b/calibrate.c index abf592a..3554295 100644 --- a/calibrate.c +++ b/calibrate.c @@ -62,27 +62,26 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int if( strcmp( so->codename, "WM0" ) == 0 || strcmp( so->codename, "WW0" ) == 0) jumpoffset += 32; else if( strcmp( so->codename, "WM1" ) == 0 ) jumpoffset += 64; - if( acode % 2 == 0 && lh == 0) //data = 0 { - bufferpts[jumpoffset*2+0][0] = (timeinsweep-100000)/500; + bufferpts[jumpoffset*2+0][0] = (timeinsweep-100000)/300; buffertimeto[jumpoffset][0] = 0; } if( acode % 2 == 1 && lh == 0 ) //data = 1 { - bufferpts[jumpoffset*2+1][0] = (timeinsweep-100000)/500; + bufferpts[jumpoffset*2+1][0] = (timeinsweep-100000)/300; buffertimeto[jumpoffset][0] = 0; } if( acode % 2 == 0 && lh == 1 ) //data = 0 { - bufferpts[jumpoffset*2+0][1] = (timeinsweep-100000)/500; + bufferpts[jumpoffset*2+0][1] = (timeinsweep-100000)/300; buffertimeto[jumpoffset][1] = 0; } if( acode % 2 == 1 && lh == 1 ) //data = 1 { - bufferpts[jumpoffset*2+1][1] = (timeinsweep-100000)/500; + bufferpts[jumpoffset*2+1][1] = (timeinsweep-100000)/300; buffertimeto[jumpoffset][1] = 0; } } -- cgit v1.2.3 From e030943790f202cc8af858135911df347a96cf8c Mon Sep 17 00:00:00 2001 From: cnlohr Date: Fri, 7 Apr 2017 00:53:11 -0400 Subject: Refactor a bit of survive_vive to re-use existing functions and simplify execution. --- src/survive_vive.c | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/survive_vive.c b/src/survive_vive.c index 1f0b5b9..8e22ea1 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -723,11 +723,9 @@ int survive_get_config( char ** config, SurviveViveData * sv, int devno, int ifa #else //Switch mode to pull config? - SV_INFO( "XXX TODO See if this can be update_feature_report" ); for( k = 0; k < 10; k++ ) { - int rk = libusb_control_transfer(dev, LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_OUT, - 0x09, 0x300 | cfgbuff_send[0], iface, cfgbuff_send, 64, 1000 ); + update_feature_report( dev, iface, cfgbuff_send, 64 ); OGUSleep(1000); } #endif -- cgit v1.2.3 From ffe28a0d0079f56191e82eb6590c59442357983a Mon Sep 17 00:00:00 2001 From: cnlohr Date: Fri, 7 Apr 2017 00:53:50 -0400 Subject: Remove feature printing out all config items at start. --- src/survive_config.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/survive_config.c b/src/survive_config.c index 005cfaf..3a83902 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -360,7 +360,10 @@ int parse_uint32(char* tag, char** values, uint16_t count) { } void handle_tag_value(char* tag, char** values, uint8_t count) { - print_json_value(tag,values,count); + + //Uncomment for more debugging of input configuration. + //print_json_value(tag,values,count); + config_group* cg = cg_stack[cg_stack_head]; if (NULL != *values){ -- cgit v1.2.3 From b93bedcc6c3fb08d028d5e787446721691b86616 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Fri, 7 Apr 2017 00:54:51 -0400 Subject: Fix warnings in dave's ortho. --- src/poser_daveortho.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index 906b21e..9d02bb3 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -460,10 +460,10 @@ PRINT(ab,2,1); #if 1 // matrix44transpose(T2, T); //Transpose so we are - matrix44copy(T2,T); + matrix44copy((FLT*)T2,(FLT*)T); cross3d( &T2[1][0], &T2[0][0], &T2[2][0] ); cross3d( &T2[2][0], &T2[1][0], &T2[0][0] ); //Replace axes in-place. - matrix44copy(T,T2); + matrix44copy((FLT*)T,(FLT*)T2); // matrix44transpose(T, T2); #endif @@ -493,7 +493,7 @@ PRINT(ab,2,1); #if 1 //matrix44copy(T2,T); - matrix44transpose(T2,T); + matrix44transpose((FLT*)T2,(FLT*)T); quatfrommatrix( quat, &T2[0][0] ); printf( "QM: %f\n", quatmagnitude( quat ) ); @@ -506,7 +506,7 @@ PRINT(ab,2,1); // PRINT(T,4,4); // matrix44copy(temp,T2); - matrix44transpose(temp,T2); + matrix44transpose((FLT*)temp,(FLT*)T2); // matrix44transpose(T2, temp); -- cgit v1.2.3 From b6787c60171724360f097a16cc7ec967995ff335 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Fri, 7 Apr 2017 00:55:21 -0400 Subject: Update, fixing charles' disambiguator, adding flag for TURVEYBIGUATOR! --- src/survive_data.c | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/src/survive_data.c b/src/survive_data.c index 3f16c7c..4bb1526 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -5,6 +5,10 @@ #include #include +#define USE_TURVEYBIGUATOR + +#ifdef USE_TURVEYBIGUATOR + typedef struct { unsigned int sweep_time[SENSORS_PER_OBJECT]; @@ -311,6 +315,9 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * le ) } + +#endif + int32_t decode_acode(uint32_t length, int32_t main_divisor) { //+50 adds a small offset and seems to help always get it right. //Check the +50 in the future to see how well this works on a variety of hardware. @@ -325,9 +332,13 @@ int32_t decode_acode(uint32_t length, int32_t main_divisor) { void handle_lightcap( SurviveObject * so, LightcapElement * le ) { SurviveContext * ctx = so->ctx; + +#ifdef USE_TURVEYBIGUATOR handle_lightcap2(so,le); return; +#else + //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; if( le->sensor_id > SENSORS_PER_OBJECT ) @@ -377,8 +388,6 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) { int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length; - - so->did_handle_ootx = 0; if( is_new_pulse ) @@ -398,6 +407,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) else { ssn = ++so->sync_set_number; + if( so->sync_set_number >= NUM_LIGHTHOUSES ) { SV_INFO( "Warning. Received an extra, unassociated sync pulse." ); @@ -432,8 +442,6 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) } } - - //See if this is a valid actual pulse. else if( le->length < so->pulse_max_for_sweep && delta > so->pulse_in_clear_time && ssn >= 0 ) { @@ -496,7 +504,6 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) so->did_handle_ootx = 1; } - if (acode > 3) { if( ssn == 0 ) { @@ -512,7 +519,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) //Make sure pulse is in valid window if( offset_from < 380000 && offset_from > 70000 ) { - ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, so->sync_set_number ); + ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, !(acode>>2) ); } } else @@ -520,6 +527,8 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) //printf( "FAIL %d %d - %d = %d\n", le->length, so->last_photo_time, le->timestamp, so->last_photo_time - le->timestamp ); //Runt pulse, or no sync pulses available. } +#endif + } -- cgit v1.2.3 From 588570a36be0cb0278edcfcdb820501d33770eef Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 7 Apr 2017 11:24:07 -0700 Subject: readme updates --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index e3b589e..3b26c5f 100644 --- a/README.md +++ b/README.md @@ -84,8 +84,8 @@ Component Type | Component | Description | Authors Poser | [poser_charlesslow.c](src/poser_charlesslow.c) | A very slow, but exhaustive poser system. Calibration only. | [@cnlohr](https://github.com/cnlohr) Poser | [poser_daveortho.c](src/poser_daveortho.c) | A very fast system using orthograpic view and affine transformations. Calibration only (for now) | [@ultramn](https://github.com/ultramn) Poser | [poser_dummy.c](src/poser_dummy.c) | Template for posers | [@cnlohr](https://github.com/cnlohr) -Poser | [poser_octavioradii.c](src/poser_octavioradii.c) | ... | [@mwturvey](https://github.com/mwturvey) and [@octavio2895](https://github.com/octavio2895) -Poser | [poser_turveytori.c](src/poser_turveytori.c) | ... | [@mwturvey](https://github.com/mwturvey) +Poser | [poser_octavioradii.c](src/poser_octavioradii.c) | A potentially very fast poser that works by finding the best fit of the distances from the lighthouse to each sensor that matches the known distances between sensors, given the known angles of a lighthouse sweep. Incomplete- distances appear to be found correctly, but more work needed to turn this into a pose. | [@mwturvey](https://github.com/mwturvey) and [@octavio2895](https://github.com/octavio2895) +Poser | [poser_turveytori.c](src/poser_turveytori.c) | A moderately fast, fairly high precision poser that works by determine the angle at the lighthouse between many sets of two sensors. Using the inscirbed angle theorom, each set defines a torus of possible locations of the lighthouse. Multiple sets define multiple tori, and this poser finds most likely location of the lighthouse using least-squares distance. Best suited for calibration, but is can be used for real-time tracking on a powerful system. | [@mwturvey](https://github.com/mwturvey) Disambiguator | [survive_data.c](src/survive_data.c) (currently #ifdefed out) | The old disambiguator - very fast, but slightly buggy. | [@cnlohr](https://github.com/cnlohr) Disambiguator | [survive_data.c](src/survive_data.c) (current disambiguator) | More complicated but much more robust disambiguator | [@mwturvey](https://github.com/mwturvey) Dismabiguator | superceded disambiguator | A more sophisticated disambiguator, development abandoned. Removed from tree. | [@jpicht](https://github.com/jpicht) -- cgit v1.2.3 From 8d33ea461c38547736f963a575dd8dbc929383d9 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sat, 8 Apr 2017 10:30:57 -0400 Subject: clarify comment --- calibrate.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/calibrate.c b/calibrate.c index 3554295..71bdcab 100644 --- a/calibrate.c +++ b/calibrate.c @@ -131,8 +131,8 @@ void * GuiThread( void * v ) uint8_t g = 0x00; uint8_t b = 0xff; - if (nn==0) b = 0; //lighthouse B - if (nn==1) r = 0; //lighthouse C + if (nn==0) b = 0; //lighthouse B, red, master + if (nn==1) r = 0; //lighthouse C, blue, slave // r = (r * (5-buffertimeto[i][nn])) / 5 ; // g = (g * (5-buffertimeto[i][nn])) / 5 ; -- cgit v1.2.3 From 454bb09f5ac0a72149c82a2231a9460e06657dea Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sat, 8 Apr 2017 10:32:59 -0400 Subject: Delay picking active lighthouse until the sweep pass so we can be sure we have the longest synce pulse time and acode. HMD reads both lighthouses. --- src/survive_data.c | 80 ++++++++++++++++++++++++++++++++++++++++++------------ 1 file changed, 62 insertions(+), 18 deletions(-) diff --git a/src/survive_data.c b/src/survive_data.c index 4bb1526..f61ae8b 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -21,8 +21,10 @@ typedef struct int activeSweepStartTime; int activeAcode; - int lh_pulse_len[NUM_LIGHTHOUSES]; +// int lh_pulse_len[NUM_LIGHTHOUSES]; int lh_start_time[NUM_LIGHTHOUSES]; + int lh_max_pulse_length[NUM_LIGHTHOUSES]; + uint16_t lh_acode[NUM_LIGHTHOUSES]; int current_lh; // used knowing which sync pulse we're looking at. } lightcap2_per_sweep_data; @@ -145,25 +147,34 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) //static unsigned int recent_sync_count = -1; //static unsigned int activeSweepStartTime; - int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); + int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); //acode for this sensor reading // Process any sweep data we have handle_lightcap2_process_sweep_data(so); int time_since_last_sync = (le->timestamp - lcd->per_sweep.recent_sync_time); - //fprintf(stderr, " %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length); + + fprintf(stderr, " %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length); // need to store up sync pulses, so we can take the earliest starting time for all sensors. if (time_since_last_sync < 2400) { lcd->per_sweep.recent_sync_time = le->timestamp; // it's the same sync pulse; - so->sync_set_number = 1; +// so->sync_set_number = 1; so->recent_sync_time = le->timestamp; - lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; - lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; +// lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; +// lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; + if (le->length > lcd->per_sweep.lh_max_pulse_length[lcd->per_sweep.current_lh]) { + lcd->per_sweep.lh_max_pulse_length[lcd->per_sweep.current_lh] = le->length; + lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; + lcd->per_sweep.lh_acode[lcd->per_sweep.current_lh] = acode; + } + +/* + //this stuff should probably be happening on the sweep so that we can filter out erroneous a codes if (!(acode >> 2 & 1)) // if the skip bit is not set { lcd->per_sweep.activeLighthouse = lcd->per_sweep.current_lh; @@ -172,19 +183,26 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) } else { + //this causes the master lighthouse to be ignored from the HMD lcd->per_sweep.activeLighthouse = -1; lcd->per_sweep.activeSweepStartTime = 0; lcd->per_sweep.activeAcode = 0; } + */ } else if (time_since_last_sync < 24000) { + lcd->per_sweep.activeLighthouse != -1; + lcd->per_sweep.recent_sync_time = le->timestamp; // I do believe we are lighthouse B lcd->per_sweep.current_lh = 1; - lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; +// lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; + lcd->per_sweep.lh_max_pulse_length[lcd->per_sweep.current_lh] = le->length; + lcd->per_sweep.lh_acode[lcd->per_sweep.current_lh] = acode; +/* if (!(acode >> 2 & 1)) // if the skip bit is not set { if (lcd->per_sweep.activeLighthouse != -1) @@ -198,10 +216,12 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) fprintf(stderr, "WARNING: Two non-skip pulses received on the same cycle!\n"); } } + lcd->per_sweep.activeLighthouse = 1; lcd->per_sweep.activeSweepStartTime = le->timestamp; lcd->per_sweep.activeAcode = acode; } + */ } else if (time_since_last_sync > 370000) @@ -210,24 +230,24 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) // first, send out the sync pulse data for the last round (for OOTX decoding { - if (lcd->per_sweep.lh_pulse_len[0] != 0) + if (lcd->per_sweep.lh_max_pulse_length[0] != 0) { so->ctx->lightproc( so, -1, - handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_pulse_len[0]), - lcd->per_sweep.lh_pulse_len[0], + handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_max_pulse_length[0]), + lcd->per_sweep.lh_max_pulse_length[0], lcd->per_sweep.lh_start_time[0], 0, 0); } - if (lcd->per_sweep.lh_pulse_len[1] != 0) + if (lcd->per_sweep.lh_max_pulse_length[1] != 0) { so->ctx->lightproc( so, -2, - handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_pulse_len[1]), - lcd->per_sweep.lh_pulse_len[1], + handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_max_pulse_length[1]), + lcd->per_sweep.lh_max_pulse_length[1], lcd->per_sweep.lh_start_time[1], 0, 1); @@ -239,23 +259,27 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) memset(&lcd->per_sweep, 0, sizeof(lcd->per_sweep)); lcd->per_sweep.activeLighthouse = -1; - - lcd->per_sweep.recent_sync_time = le->timestamp; // I do believe we are lighthouse A lcd->per_sweep.current_lh = 0; - lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; +// lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; + lcd->per_sweep.lh_max_pulse_length[lcd->per_sweep.current_lh] = le->length; + lcd->per_sweep.lh_acode[lcd->per_sweep.current_lh] = acode; - int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); +// int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); +/* if (!(acode >> 2 & 1)) // if the skip bit is not set { lcd->per_sweep.activeLighthouse = 0; lcd->per_sweep.activeSweepStartTime = le->timestamp; lcd->per_sweep.activeAcode = acode; } + */ } + +// printf("%d %d\n", acode, lcd->per_sweep.activeLighthouse ); } void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) @@ -275,6 +299,25 @@ void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) //fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); //fprintf(stderr, "."); + lcd->per_sweep.activeLighthouse = -1; + lcd->per_sweep.activeSweepStartTime = 0; + lcd->per_sweep.activeAcode = 0; + + for (uint8_t i=0; i< NUM_LIGHTHOUSES;++i) { + int acode = lcd->per_sweep.lh_acode[i]; + if ((acode>0) && (!(acode >> 2 & 1))) { + lcd->per_sweep.activeLighthouse = i; + lcd->per_sweep.activeSweepStartTime = lcd->per_sweep.lh_start_time[i]; + lcd->per_sweep.activeAcode = acode; + } + } + + if (lcd->per_sweep.activeLighthouse < 0) { + fprintf(stderr, "WARNING: No active lighthouse!\n"); + fprintf(stderr, " %2d %8d\n", le->sensor_id, le->length); + return; + } + if (lcd->sweep.sweep_len[le->sensor_id] < le->length) { lcd->sweep.sweep_len[le->sensor_id] = le->length; @@ -303,7 +346,8 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * le ) // Should never get a reading so high. Odd. return; } - if (le->length >= 2750) +// if (le->length >= 2750) + if (le->length >= 2500) { // Looks like a sync pulse, process it! handle_lightcap2_sync(so, le); -- cgit v1.2.3 From 96cec93d3c7461587673b5dc35c6fba1e9db899f Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sat, 8 Apr 2017 13:36:42 -0400 Subject: clear acode at beginning of pulses. only process valid acodes sweeps. makes HMD work again with single or double lighthouses. --- src/survive_data.c | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/src/survive_data.c b/src/survive_data.c index f61ae8b..989eb59 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -24,7 +24,7 @@ typedef struct // int lh_pulse_len[NUM_LIGHTHOUSES]; int lh_start_time[NUM_LIGHTHOUSES]; int lh_max_pulse_length[NUM_LIGHTHOUSES]; - uint16_t lh_acode[NUM_LIGHTHOUSES]; + int8_t lh_acode[NUM_LIGHTHOUSES]; int current_lh; // used knowing which sync pulse we're looking at. } lightcap2_per_sweep_data; @@ -96,7 +96,8 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) { static int counts[SENSORS_PER_OBJECT][2] = {0}; - if (lcd->per_sweep.activeLighthouse == 0 && !allZero) +// if (lcd->per_sweep.activeLighthouse == 0 && !allZero) + if (lcd->per_sweep.activeLighthouse > -1 && !allZero) { if (lcd->sweep.sweep_len[i] != 0) { @@ -226,6 +227,9 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) } else if (time_since_last_sync > 370000) { + // XXX CAUTION: if we lose sight of a lighthouse then, the remaining lighthouse will default to master + //this should probably be fixed. Maybe some kind of timing based guess at which lighthouse. + // looks like this is the first sync pulse. Cool! // first, send out the sync pulse data for the last round (for OOTX decoding @@ -259,6 +263,10 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) memset(&lcd->per_sweep, 0, sizeof(lcd->per_sweep)); lcd->per_sweep.activeLighthouse = -1; + for (uint8_t i=0; i < NUM_LIGHTHOUSES;++i) { + lcd->per_sweep.lh_acode[i] = -1; + } + lcd->per_sweep.recent_sync_time = le->timestamp; // I do believe we are lighthouse A lcd->per_sweep.current_lh = 0; @@ -303,9 +311,9 @@ void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) lcd->per_sweep.activeSweepStartTime = 0; lcd->per_sweep.activeAcode = 0; - for (uint8_t i=0; i< NUM_LIGHTHOUSES;++i) { + for (uint8_t i=0; i < NUM_LIGHTHOUSES;++i) { int acode = lcd->per_sweep.lh_acode[i]; - if ((acode>0) && (!(acode >> 2 & 1))) { + if ( (acode>=0) && !(acode >> 2 & 1)) { lcd->per_sweep.activeLighthouse = i; lcd->per_sweep.activeSweepStartTime = lcd->per_sweep.lh_start_time[i]; lcd->per_sweep.activeAcode = acode; @@ -314,7 +322,7 @@ void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) if (lcd->per_sweep.activeLighthouse < 0) { fprintf(stderr, "WARNING: No active lighthouse!\n"); - fprintf(stderr, " %2d %8d\n", le->sensor_id, le->length); + fprintf(stderr, " %2d %8d %d %d\n", le->sensor_id, le->length,lcd->per_sweep.lh_acode[0],lcd->per_sweep.lh_acode[1]); return; } -- cgit v1.2.3 From 1398b5dd201a69d4c769adce4bf4eabb5e3a28fa Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sat, 8 Apr 2017 17:40:56 -0400 Subject: don't paint 0 coordinates --- calibrate.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/calibrate.c b/calibrate.c index 71bdcab..67c1d27 100644 --- a/calibrate.c +++ b/calibrate.c @@ -138,6 +138,9 @@ void * GuiThread( void * v ) // g = (g * (5-buffertimeto[i][nn])) / 5 ; // b = (b * (5-buffertimeto[i][nn])) / 5 ; CNFGColor( (b<<16) | (g<<8) | r ); + + if (bufferpts[i*2+0][nn] == 0 || bufferpts[i*2+1][nn]==0) continue; //do not draw if aither coordinate is 0 + CNFGTackRectangle( bufferpts[i*2+0][nn], bufferpts[i*2+1][nn], bufferpts[i*2+0][nn] + 5, bufferpts[i*2+1][nn] + 5 ); CNFGPenX = bufferpts[i*2+0][nn]; CNFGPenY = bufferpts[i*2+1][nn]; CNFGDrawText( buffermts, 2 ); -- cgit v1.2.3 From e2d553191e8693bb8937aac8b991d9c09e0df6cc Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sat, 8 Apr 2017 17:42:28 -0400 Subject: try to remove outliers --- src/survive_data.c | 63 +++++++++++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 60 insertions(+), 3 deletions(-) diff --git a/src/survive_data.c b/src/survive_data.c index 989eb59..647a5d7 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -65,10 +65,64 @@ int handle_lightcap2_getAcodeFromSyncPulse(SurviveObject * so, int pulseLen) if (pulseLen < 6250 - ACODE_OFFSET) return 6; return 7; } + +uint8_t remove_outliers(SurviveObject *so) { + lightcap2_data *lcd = so->disambiguator_data; + + uint32_t sum = 0; + uint8_t non_zero_count = 0; + uint32_t mean = 0; + + for (int i = 0; i < SENSORS_PER_OBJECT; i++) + { + if (lcd->sweep.sweep_len[i] > 0) { + sum += lcd->sweep.sweep_len[i]; + ++non_zero_count; + } + } + + if (non_zero_count==0) return 0; + + mean = sum/non_zero_count; + + float standard_deviation = 0.0f; + sum = 0; + for (int i = 0; i < SENSORS_PER_OBJECT; i++) + { + uint16_t len = lcd->sweep.sweep_len[i]; + if (len > 0) { + sum += (len - mean)*(len - mean); + } + } + standard_deviation = sqrt( ((float)sum)/((float)non_zero_count) ); + +// printf("%f\n", standard_deviation); + + float fake_tao_test = standard_deviation*2; + uint8_t removed_outliers = 0; + + for (int i = 0; i < SENSORS_PER_OBJECT; i++) + { + uint16_t len = lcd->sweep.sweep_len[i]; + if (len == 0) continue; + + if ( abs(len-mean) > fake_tao_test ) + { +// fprintf(stderr, "removing %d\n", len); + lcd->sweep.sweep_len[i] = 0; + removed_outliers = 1; + } + } + + return removed_outliers; +} + void handle_lightcap2_process_sweep_data(SurviveObject *so) { lightcap2_data *lcd = so->disambiguator_data; + while(remove_outliers(so)); + // look at all of the sensors we found, and process the ones that were hit. // TODO: find the sensor(s) with the longest pulse length, and assume // those are the "highest quality". Then, reject any pulses that are sufficiently @@ -93,6 +147,7 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) // printf("a[%d]l[%d] ", lcd->per_sweep.activeAcode & 5, lcd->per_sweep.activeLighthouse); for (int i = 0; i < SENSORS_PER_OBJECT; i++) { + { static int counts[SENSORS_PER_OBJECT][2] = {0}; @@ -119,10 +174,10 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) if (lcd->sweep.sweep_len[i] != 0) // if the sensor was hit, process it { - +//printf("%4d\n", lcd->sweep.sweep_len[i]); int offset_from = lcd->sweep.sweep_time[i] - lcd->per_sweep.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2; - if (offset_from < 380000 && offset_from > 70000) +// if (offset_from < 380000 && offset_from > 70000) { //if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i]) { @@ -134,7 +189,9 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) //if (!allZero) // printf(" ..:..\n"); +// if (!allZero) printf("\n"); } + // clear out sweep data (could probably limit this to only after a "first" sync. // this is slightly more robust, so doing it here for now. memset(&(((lightcap2_data*)so->disambiguator_data)->sweep), 0, sizeof(lightcaps_sweep_data)); @@ -156,7 +213,7 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) int time_since_last_sync = (le->timestamp - lcd->per_sweep.recent_sync_time); - fprintf(stderr, " %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length); +// fprintf(stderr, " %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length); // need to store up sync pulses, so we can take the earliest starting time for all sensors. if (time_since_last_sync < 2400) { -- cgit v1.2.3 From 7686ad97d5f2542338f88e6f6e8055953609651c Mon Sep 17 00:00:00 2001 From: Josh Allen Date: Sun, 9 Apr 2017 08:38:40 -0400 Subject: untested tau test --- src/survive_data.c | 68 ++++++++++++++++++++++++++++++++++++++++++------------ 1 file changed, 53 insertions(+), 15 deletions(-) diff --git a/src/survive_data.c b/src/survive_data.c index 647a5d7..8d115fd 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -4,16 +4,25 @@ #include "survive_internal.h" #include #include +#include #define USE_TURVEYBIGUATOR #ifdef USE_TURVEYBIGUATOR +static const float tau_table[33] = { 0, 0, 0, 1.151140982, 1.425, 1.5712213707, 1.656266074, 1.7110275587, 1.7490784054, + 1.7770229476, 1.798410005, 1.8153056661, 1.8289916275, 1.8403044103, 1.8498129961, 1.8579178211, + 1.864908883, 1.8710013691, 1.8763583296, 1.881105575, 1.885341741, 1.8891452542, 1.8925792599, + 1.8956951735, 1.8985352854, 1.9011347009, 1.9035228046, 1.9057243816, 1.9077604832, 1.9096491058, + 1.9114057255, 1.9130437248, 1.914574735 +}; + typedef struct { unsigned int sweep_time[SENSORS_PER_OBJECT]; - unsigned int sweep_len[SENSORS_PER_OBJECT]; + uint16_t sweep_len[SENSORS_PER_OBJECT]; //might want to align this to cache lines, will be hot for frequent access } lightcaps_sweep_data; + typedef struct { int recent_sync_time; @@ -73,12 +82,16 @@ uint8_t remove_outliers(SurviveObject *so) { uint8_t non_zero_count = 0; uint32_t mean = 0; - for (int i = 0; i < SENSORS_PER_OBJECT; i++) + uint16_t* min = NULL; + uint16_t* max = NULL; + uint8_t found_first = 0; + + //future: https://gcc.gnu.org/projects/tree-ssa/vectorization.html#vectorizab + + for (uint8_t i = 0; i < SENSORS_PER_OBJECT; i++) { - if (lcd->sweep.sweep_len[i] > 0) { - sum += lcd->sweep.sweep_len[i]; - ++non_zero_count; - } + sum += lcd->sweep.sweep_len[i]; + if (lcd->sweep.sweep_len[i] > 0) ++non_zero_count; } if (non_zero_count==0) return 0; @@ -87,34 +100,59 @@ uint8_t remove_outliers(SurviveObject *so) { float standard_deviation = 0.0f; sum = 0; - for (int i = 0; i < SENSORS_PER_OBJECT; i++) + for (uint8_t i = 0; i < SENSORS_PER_OBJECT; i++) { uint16_t len = lcd->sweep.sweep_len[i]; if (len > 0) { sum += (len - mean)*(len - mean); + + if (found_first==0) { + max = min = lcd->sweep.sweep_len + i; + found_first=1; + } else { + if(lcd->sweep.sweep_len[i] < *min) min=lcd->sweep.sweep_len + i; + if(lcd->sweep.sweep_len[i] > *max) max=lcd->sweep.sweep_len + i; + } } } - standard_deviation = sqrt( ((float)sum)/((float)non_zero_count) ); + standard_deviation = sqrtf( ((float)sum)/((float)non_zero_count) ); // printf("%f\n", standard_deviation); - float fake_tao_test = standard_deviation*2; - uint8_t removed_outliers = 0; + float tau_test = standard_deviation; + + if (non_zero_count > 2) tau_test = standard_deviation*tau_table[non_zero_count]; - for (int i = 0; i < SENSORS_PER_OBJECT; i++) +// uint8_t removed_outliers = 0; + + uint32_t d1 = *min - mean; + uint32_t d2 = *max - mean; + + if (d1>d2 && d1>tau_test) { + *min = 0; + return 1; + } + else if (d2>tau_test) { + *max = 0; + return 1; + } + + return 0; +/* + for (uint8_t i = 0; i < SENSORS_PER_OBJECT; i++) { uint16_t len = lcd->sweep.sweep_len[i]; if (len == 0) continue; - if ( abs(len-mean) > fake_tao_test ) + if ( abs(len-mean) > tau_test ) { // fprintf(stderr, "removing %d\n", len); lcd->sweep.sweep_len[i] = 0; removed_outliers = 1; } } - - return removed_outliers; +*/ +// return removed_outliers; } void handle_lightcap2_process_sweep_data(SurviveObject *so) @@ -250,7 +288,7 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) } else if (time_since_last_sync < 24000) { - lcd->per_sweep.activeLighthouse != -1; + lcd->per_sweep.activeLighthouse = -1; lcd->per_sweep.recent_sync_time = le->timestamp; // I do believe we are lighthouse B -- cgit v1.2.3 From 8dee94008fec1506a3a2f58292a45e1f93b6d333 Mon Sep 17 00:00:00 2001 From: Josh Allen Date: Sun, 9 Apr 2017 08:51:29 -0400 Subject: should be absolute value if statement should not be combined --- src/survive_data.c | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/survive_data.c b/src/survive_data.c index 8d115fd..22ce8c2 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -125,12 +125,14 @@ uint8_t remove_outliers(SurviveObject *so) { // uint8_t removed_outliers = 0; - uint32_t d1 = *min - mean; - uint32_t d2 = *max - mean; + uint32_t d1 = abs(*min - mean); + uint32_t d2 = abs(*max - mean); - if (d1>d2 && d1>tau_test) { - *min = 0; - return 1; + if (d1>d2) { + if (d1 > tau_test) { + *min = 0; + return 1; + } } else if (d2>tau_test) { *max = 0; -- cgit v1.2.3 From 6423c27236d7ee3bfaac293990ef78779507bb02 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Tue, 11 Apr 2017 10:57:58 -0700 Subject: Perf: Take advantage of past poses for future estimates --- src/poser_turveytori.c | 36 ++++++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index c251040..5ad5c1b 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -83,6 +83,9 @@ typedef struct FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance int angleIndex[NUM_LIGHTHOUSES][2]; // index into circular buffer ahead. separate index for each axis. int lastAxis[NUM_LIGHTHOUSES]; + + Point lastLhPos[NUM_LIGHTHOUSES]; + FLT lastLhRotAxisAngle[NUM_LIGHTHOUSES][4]; } ToriData; @@ -1144,6 +1147,8 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, char doLogOutput, int lh, int setLhCalibration) { + ToriData *toriData = so->PoserData; + //printf("Solving for Lighthouse\n"); //printf("obj->numSensors = %d;\n", obj->numSensors); @@ -1234,6 +1239,14 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob // back into the search for the correct point (see "if (a1 > M_PI / 2)" below) Point p1 = getNormalizedAndScaledVector(avgNorm, 8); + // if the last lighthouse position has been populated (extremely rare it would be 0) + if (toriData->lastLhPos[lh].x != 0) + { + p1.x = toriData->lastLhPos[lh].x; + p1.y = toriData->lastLhPos[lh].y; + p1.z = toriData->lastLhPos[lh].z; + } + Point refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p1, pna, pnaCount, logFile); FLT pf1[3] = { refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z }; @@ -1258,11 +1271,29 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob //printf("Distance is %f, Fitness is %f\n", distance, fitGd); FLT rot[4]; // this is axis/ angle rotation, not a quaternion! + + if (toriData->lastLhRotAxisAngle[lh][0] != 0) + { + rot[0] = toriData->lastLhRotAxisAngle[lh][0]; + rot[1] = toriData->lastLhRotAxisAngle[lh][1]; + rot[2] = toriData->lastLhRotAxisAngle[lh][2]; + rot[3] = toriData->lastLhRotAxisAngle[lh][3]; + } + + SolveForRotation(rot, obj, refinedEstimateGd); FLT objPos[3]; + { + toriData->lastLhRotAxisAngle[lh][0] = rot[0]; + toriData->lastLhRotAxisAngle[lh][1] = rot[1]; + toriData->lastLhRotAxisAngle[lh][2] = rot[2]; + toriData->lastLhRotAxisAngle[lh][3] = rot[3]; + } + WhereIsTheTrackedObjectAxisAngle(objPos, rot, refinedEstimateGd); + FLT rotQuat[4]; quatfromaxisangle(rotQuat, rot, rot[3]); @@ -1325,6 +1356,11 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob fclose(logFile); } + + toriData->lastLhPos[lh].x = refinedEstimateGd.x; + toriData->lastLhPos[lh].y = refinedEstimateGd.y; + toriData->lastLhPos[lh].z = refinedEstimateGd.z; + return refinedEstimateGd; } -- cgit v1.2.3 From 30531472d0afccdb3b06b2e8fe8894ef4f9440be Mon Sep 17 00:00:00 2001 From: mwturvey Date: Tue, 11 Apr 2017 11:59:06 -0700 Subject: Improve performance of gradient calculation --- src/poser_turveytori.c | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 5ad5c1b..c526161 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -436,23 +436,25 @@ Point getGradient(Point pointIn, PointsAndAngle *pna, size_t pnaCount, FLT preci { Point result; + FLT baseFitness = getPointFitness(pointIn, pna, pnaCount, 0); + Point tmpXplus = pointIn; Point tmpXminus = pointIn; tmpXplus.x = pointIn.x + precision; tmpXminus.x = pointIn.x - precision; - result.x = getPointFitness(tmpXplus, pna, pnaCount, 0) - getPointFitness(tmpXminus, pna, pnaCount, 0); + result.x = baseFitness - getPointFitness(tmpXminus, pna, pnaCount, 0); Point tmpYplus = pointIn; Point tmpYminus = pointIn; tmpYplus.y = pointIn.y + precision; tmpYminus.y = pointIn.y - precision; - result.y = getPointFitness(tmpYplus, pna, pnaCount, 0) - getPointFitness(tmpYminus, pna, pnaCount, 0); + result.y = baseFitness - getPointFitness(tmpYminus, pna, pnaCount, 0); Point tmpZplus = pointIn; Point tmpZminus = pointIn; tmpZplus.z = pointIn.z + precision; tmpZminus.z = pointIn.z - precision; - result.z = getPointFitness(tmpZplus, pna, pnaCount, 0) - getPointFitness(tmpZminus, pna, pnaCount, 0); + result.z = baseFitness - getPointFitness(tmpZminus, pna, pnaCount, 0); return result; } @@ -1518,7 +1520,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) counter++; // let's just do this occasionally for now... - if (counter % 2 == 0) + //if (counter % 1 == 0) QuickPose(so, 0); } // axis changed, time to increment the circular buffer index. -- cgit v1.2.3 From 0b16cebf7ca20da43da99cf9d59ab9ed8a3c205f Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Tue, 11 Apr 2017 17:38:15 -0700 Subject: Better messaging when finding a device --- src/survive_vive.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/survive_vive.c b/src/survive_vive.c index 8e22ea1..3ce985d 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -54,7 +54,7 @@ const short vidpids[] = { const char * devnames[] = { "HMD", - "Lighthouse", + "HMD IMU & LH", "Watchman 1", "Watchman 2", "Tracker 0", @@ -370,6 +370,7 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * wchar_t wstr[255]; res = hid_get_serial_number_string(handle, wstr, 255); + printf("Found %s. ", devnames[i]); wprintf(L"Serial Number String: (%d) %s for %04x:%04x@%d (Dev: %p)\n", wstr[0], wstr,vendor_id, product_id, menum, handle); sv->udev[i] = handle; -- cgit v1.2.3 From 4b842a1a73a2713726c3d999ab4b6886e7e9fb69 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Tue, 11 Apr 2017 17:42:04 -0700 Subject: Disabling remove_outliers because it's removing too many non-outliers --- src/survive_data.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/survive_data.c b/src/survive_data.c index 647a5d7..0273532 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -67,6 +67,7 @@ int handle_lightcap2_getAcodeFromSyncPulse(SurviveObject * so, int pulseLen) } uint8_t remove_outliers(SurviveObject *so) { + return 0; // disabling this for now because it seems remove almost all the points for wired watchman and wired tracker. lightcap2_data *lcd = so->disambiguator_data; uint32_t sum = 0; -- cgit v1.2.3 From 06f580d722136051c55653f6767f185b396f3371 Mon Sep 17 00:00:00 2001 From: "Dr. Orion Lawlor" Date: Mon, 17 Apr 2017 21:38:46 -0800 Subject: Support calling the library from C++, by adding extern "C" decorations, and including correct prototypes. --- include/libsurvive/poser.h | 9 +++++++++ include/libsurvive/survive.h | 13 ++++++++++++- include/libsurvive/survive_types.h | 8 ++++++++ redist/os_generic.c | 17 +++++++++-------- 4 files changed, 38 insertions(+), 9 deletions(-) diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h index 497b009..582590e 100644 --- a/include/libsurvive/poser.h +++ b/include/libsurvive/poser.h @@ -3,6 +3,11 @@ #include "survive_types.h" + +#ifdef __cplusplus +extern "C" { +#endif + typedef enum PoserType_t { POSERDATA_NONE = 0, @@ -55,4 +60,8 @@ typedef struct typedef int (*PoserCB)( SurviveObject * so, PoserData * pd ); +#ifdef __cplusplus +}; +#endif + #endif diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 278bbca..3c35bd2 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -5,6 +5,10 @@ #include "survive_types.h" #include "poser.h" +#ifdef __cplusplus +extern "C" { +#endif + //DANGER: This structure may be redefined. Note that it is logically split into 64-bit chunks //for optimization on 32- and 64-bit systems. @@ -116,7 +120,7 @@ void survive_install_imu_fn( SurviveContext * ctx, imu_process_func fbp ); void survive_install_angle_fn( SurviveContext * ctx, angle_process_func fbp ); void survive_close( SurviveContext * ctx ); -int survive_poll(); +int survive_poll( SurviveContext * ctx ); SurviveObject * survive_get_so_by_name( SurviveContext * ctx, const char * name ); @@ -128,6 +132,9 @@ int survive_send_magic( SurviveContext * ctx, int magic_code, void * data, int d //Install the calibrator. void survive_cal_install( SurviveContext * ctx ); //XXX This will be removed if not already done so. +// Read back a human-readable string description of the calibration status +int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length ); + //Call these from your callback if overridden. //Accept higher-level data. void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length , uint32_t lh); @@ -174,5 +181,9 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ); #define SV_ERROR( ... ) { char stbuff[1024]; sprintf( stbuff, __VA_ARGS__ ); ctx->faultfunction( ctx, stbuff ); } #define SV_KILL() exit(0) //XXX This should likely be re-defined. +#ifdef __cplusplus +}; +#endif + #endif diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index bfd0b1d..224719e 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -1,6 +1,11 @@ #ifndef _SURVIVE_TYPES_H #define _SURVIVE_TYPES_H +#ifdef __cplusplus +extern "C" { +#endif + + #ifndef FLT #ifdef USE_DOUBLE #define FLT double @@ -39,6 +44,9 @@ typedef int (*DeviceDriver)( SurviveContext * ctx ); typedef int (*DeviceDriverCb)( struct SurviveContext * ctx, void * driver ); typedef int (*DeviceDriverMagicCb)( struct SurviveContext * ctx, void * driver, int magic_code, void * data, int datalen ); +#ifdef __cplusplus +}; +#endif #endif diff --git a/redist/os_generic.c b/redist/os_generic.c index 1ab4863..3191357 100644 --- a/redist/os_generic.c +++ b/redist/os_generic.c @@ -151,8 +151,9 @@ void OGDeleteSema( og_sema_t os ) #else -#define _GNU_SOURCE - +#ifndef _GNU_SOURCE +# define _GNU_SOURCE +#endif #include #include @@ -198,7 +199,7 @@ double OGGetFileTime( const char * file ) og_thread_t OGCreateThread( void * (routine)( void * ), void * parameter ) { - pthread_t * ret = malloc( sizeof( pthread_t ) ); + pthread_t * ret = (pthread_t *)malloc( sizeof( pthread_t ) ); int r = pthread_create( ret, 0, routine, parameter ); if( r ) { @@ -277,7 +278,7 @@ void OGDeleteMutex( og_mutex_t om ) og_sema_t OGCreateSema() { - sem_t * sem = malloc( sizeof( sem_t ) ); + sem_t * sem = (sem_t *)malloc( sizeof( sem_t ) ); sem_init( sem, 0, 0 ); return (og_sema_t)sem; } @@ -285,24 +286,24 @@ og_sema_t OGCreateSema() int OGGetSema( og_sema_t os ) { int valp; - sem_getvalue( os, &valp ); + sem_getvalue( (sem_t *)os, &valp ); return valp; } void OGLockSema( og_sema_t os ) { - sem_wait( os ); + sem_wait( (sem_t *)os ); } void OGUnlockSema( og_sema_t os ) { - sem_post( os ); + sem_post( (sem_t *)os ); } void OGDeleteSema( og_sema_t os ) { - sem_destroy( os ); + sem_destroy( (sem_t *)os ); free(os); } -- cgit v1.2.3 From a7d4ccbde10be9c14ae690d3c9a21627195cfaa5 Mon Sep 17 00:00:00 2001 From: "Dr. Orion Lawlor" Date: Mon, 17 Apr 2017 21:42:05 -0800 Subject: Fix warnings by including --- src/survive_data.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/survive_data.c b/src/survive_data.c index 0273532..7b9d051 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -4,6 +4,7 @@ #include "survive_internal.h" #include #include +#include /* for sqrt */ #define USE_TURVEYBIGUATOR -- cgit v1.2.3 From 5e934529f953794f7d8a070766fd56e02eeb8b05 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 29 Apr 2017 15:59:58 -0400 Subject: Allow modificaiton of .o files for inclusion of different features. --- Makefile | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/Makefile b/Makefile index 1899c4c..22cbdcb 100644 --- a/Makefile +++ b/Makefile @@ -28,9 +28,6 @@ endif POSERS:=src/poser_dummy.o src/poser_daveortho.o src/poser_charlesslow.o REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/os_generic.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 src/survive_cal.o -LIBSURVIVE_CORE:=$(LIBSURVIVE_CORE) -LIBSURVIVE_O:=$(POSERS) $(REDISTS) $(LIBSURVIVE_CORE) -LIBSURVIVE_C:=$(LIBSURVIVE_O:.o=.c) #If you want to use HIDAPI on Linux. @@ -48,6 +45,11 @@ LIBSURVIVE_C:=$(LIBSURVIVE_O:.o=.c) + +LIBSURVIVE_CORE:=$(LIBSURVIVE_CORE) +LIBSURVIVE_O:=$(POSERS) $(REDISTS) $(LIBSURVIVE_CORE) +LIBSURVIVE_C:=$(LIBSURVIVE_O:.o=.c) + # unused: redist/crc32.c test : test.c ./lib/libsurvive.so redist/os_generic.o -- cgit v1.2.3 From 57feef80252b28362b3adbee1e587467b3f329b0 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 29 Apr 2017 17:10:40 -0400 Subject: cleanup warnings --- redist/json_helpers.c | 28 +++++++++++++++------------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/redist/json_helpers.c b/redist/json_helpers.c index 29d48bd..c52301d 100644 --- a/redist/json_helpers.c +++ b/redist/json_helpers.c @@ -50,44 +50,46 @@ void json_write_float_array(FILE* f, const char* tag, float* v, uint8_t count) { uint8_t i = 0; char * str1 = NULL; char * str2 = NULL; - asprintf(&str1,"\"%s\":[", tag); + if( asprintf(&str1,"\"%s\":[", tag) < 0 ) goto giveup; for (i=0;i Date: Sat, 29 Apr 2017 17:10:48 -0400 Subject: Add extra posers --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index 22cbdcb..54fd6ee 100644 --- a/Makefile +++ b/Makefile @@ -25,7 +25,7 @@ GRAPHICS_LOFI:=redist/CNFGFunctions.o redist/CNFGXDriver.o endif -POSERS:=src/poser_dummy.o src/poser_daveortho.o src/poser_charlesslow.o +POSERS:=src/poser_dummy.o src/poser_daveortho.o src/poser_charlesslow.o src/poser_octavioradii.o src/poser_turveytori.o REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/os_generic.o 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 -- cgit v1.2.3 From 1a6f5f8316b2dfa1baa923f5ef109b7a4890300f Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 29 Apr 2017 17:44:22 -0400 Subject: Fix survive_vive's accidental collection of code 0xfe data elements. --- src/survive_vive.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/survive_vive.c b/src/survive_vive.c index 3ce985d..4acc4e4 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1165,7 +1165,7 @@ void survive_data_cb( SurviveUSBInterface * si ) le.sensor_id = POP1; le.length = POP2; le.timestamp = POP4; - if( le.sensor_id == 0xff ) break; + if( le.sensor_id > 0xfd ) break; handle_lightcap( obj, &le ); } break; @@ -1183,7 +1183,7 @@ void survive_data_cb( SurviveUSBInterface * si ) le.sensor_id = (uint8_t)POP2; le.length = POP2; le.timestamp = POP4; - if( le.sensor_id == 0xff ) break; + if( le.sensor_id > 0xfd ) break; handle_lightcap( obj, &le ); } break; -- cgit v1.2.3 From 2f7ebf7af36e324d8f1d8036800d34eeb2d9f46f Mon Sep 17 00:00:00 2001 From: CNLohr Date: Sat, 29 Apr 2017 18:03:50 -0400 Subject: Add a nomenclature section to the readme. --- README.md | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/README.md b/README.md index 3b26c5f..dc568f6 100644 --- a/README.md +++ b/README.md @@ -26,6 +26,22 @@ Please see the issues for what help needs to be done now! HackADay article and video with Dr. Yates on how they made the Vive a thing. http://hackaday.com/2016/12/21/alan-yates-why-valves-lighthouse-cant-work/ + +## Nomenclature + +WRT = With Respect To +PoV / POV = Point of View (typically WRT to a LH, sometimes (though rarely) a sensor) +LH = Lighthouse = Base Station = A device that produces a 1.8 MHz modulated sync pulse in IR and then sweeps the scene with laser planes. +Sync Pulse = A pulse of modulated IR data sent from a ligthhouse, typically by the floodlight aspect of a lighthouse. +Sweep Pulse = The evenlope created by a laser sweeping over a light sensor. +OOTX = Omnidirectional Optical Transmitter = Data encoded in the sync pulses of the LHs. +HMD = Headset = Main sensor receiver with a visual display for a human. +WM = Watchman = Controller = The HTC Vive controller. +TR = Tracker = Official HTC Tracker. +LightcapElement = A single pulse of light, including a timestamp, source sensor and length of pulse. +Disambiguator = System that accepts lightcap elements and pulls out OOTX data and relative sweep times of sweep pulses. +Poser = Device to convert series of angles from a LH's PoV + ## Getting things working There are two things you should consider doing to your system before running libsurvive. -- cgit v1.2.3 From 24962ebf94c6134081c41f95acc1461d8a5bf242 Mon Sep 17 00:00:00 2001 From: CNLohr Date: Sat, 29 Apr 2017 18:06:20 -0400 Subject: List out Nomenclature. --- README.md | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/README.md b/README.md index dc568f6..130fa1e 100644 --- a/README.md +++ b/README.md @@ -29,18 +29,18 @@ HackADay article and video with Dr. Yates on how they made the Vive a thing. htt ## Nomenclature -WRT = With Respect To -PoV / POV = Point of View (typically WRT to a LH, sometimes (though rarely) a sensor) -LH = Lighthouse = Base Station = A device that produces a 1.8 MHz modulated sync pulse in IR and then sweeps the scene with laser planes. -Sync Pulse = A pulse of modulated IR data sent from a ligthhouse, typically by the floodlight aspect of a lighthouse. -Sweep Pulse = The evenlope created by a laser sweeping over a light sensor. -OOTX = Omnidirectional Optical Transmitter = Data encoded in the sync pulses of the LHs. -HMD = Headset = Main sensor receiver with a visual display for a human. -WM = Watchman = Controller = The HTC Vive controller. -TR = Tracker = Official HTC Tracker. -LightcapElement = A single pulse of light, including a timestamp, source sensor and length of pulse. -Disambiguator = System that accepts lightcap elements and pulls out OOTX data and relative sweep times of sweep pulses. -Poser = Device to convert series of angles from a LH's PoV +* WRT = With Respect To +* PoV / POV = Point of View (typically WRT to a LH, sometimes (though rarely) a sensor) +* LH = Lighthouse = Base Station = A device that produces a 1.8 MHz modulated sync pulse in IR and then sweeps the scene with laser planes. +* Sync Pulse = A pulse of modulated IR data sent from a ligthhouse, typically by the floodlight aspect of a lighthouse. +* Sweep Pulse = The evenlope created by a laser sweeping over a light sensor. +* OOTX = Omnidirectional Optical Transmitter = Data encoded in the sync pulses of the LHs. +* HMD = Headset = Main sensor receiver with a visual display for a human. +* WM = Watchman = Controller = The HTC Vive controller. +* TR = Tracker = Official HTC Tracker. +* LightcapElement = A single pulse of light, including a timestamp, source sensor and length of pulse. +* Disambiguator = System that accepts lightcap elements and pulls out OOTX data and relative sweep times of sweep pulses. +* Poser = Device to convert series of angles from a LH's PoV ## Getting things working -- cgit v1.2.3 From 155124e6ac5cba02bb6370779ed3cc35bccd9d01 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 30 Apr 2017 00:10:55 -0400 Subject: Fix mistaken edit to survive_vive which dropped rest of packet when invalid sensor was found. Feels odd that this is actually an issue. --- src/survive_vive.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/survive_vive.c b/src/survive_vive.c index 4acc4e4..398ff99 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1165,7 +1165,7 @@ void survive_data_cb( SurviveUSBInterface * si ) le.sensor_id = POP1; le.length = POP2; le.timestamp = POP4; - if( le.sensor_id > 0xfd ) break; + if( le.sensor_id > 0xfd ) continue; handle_lightcap( obj, &le ); } break; @@ -1183,7 +1183,7 @@ void survive_data_cb( SurviveUSBInterface * si ) le.sensor_id = (uint8_t)POP2; le.length = POP2; le.timestamp = POP4; - if( le.sensor_id > 0xfd ) break; + if( le.sensor_id > 0xfd ) continue; // handle_lightcap( obj, &le ); } break; -- cgit v1.2.3 From f177f87e98637de8f07488d820390f322c45413c Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 30 Apr 2017 00:13:42 -0400 Subject: Update the charles disambiguator --- include/libsurvive/survive.h | 2 +- src/survive_data.c | 53 +++++++++++++++++++++++++++++++------------- 2 files changed, 39 insertions(+), 16 deletions(-) diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 3c35bd2..e85fe48 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -42,7 +42,7 @@ struct SurviveObject //Timing sensitive data (mostly for disambiguation) int32_t timebase_hz; //48,000,000 for normal vive hardware. (checked) - int32_t timecenter_ticks; //200,000 for normal vive hardware. (checked) + int32_t timecenter_ticks; //200,000 for normal vive hardware. (checked) (This doubles-up as 2x this = full sweep length) int32_t pulsedist_max_ticks; //500,000 for normal vive hardware. (guessed) int32_t pulselength_min_sync; //2,200 for normal vive hardware. (guessed) int32_t pulse_in_clear_time; //35,000 for normal vive hardware. (guessed) diff --git a/src/survive_data.c b/src/survive_data.c index 7b9d051..0e2e828 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -6,7 +6,7 @@ #include #include /* for sqrt */ -#define USE_TURVEYBIGUATOR +//#define USE_TURVEYBIGUATOR #ifdef USE_TURVEYBIGUATOR @@ -449,6 +449,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) return; #else + //printf( "LE%3d%6d%12d\n", le->sensor_id, le->length, le->timestamp ); //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; @@ -482,6 +483,9 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) //unified driver. int ssn = so->sync_set_number; //lighthouse number if( ssn < 0 ) ssn = 0; +#ifdef DEBUG + if( ssn >= NUM_LIGHTHOUSES ) { SV_INFO( "ALGORITHMIC WARNING: ssn exceeds NUM_LIGHTHOUSES" ); } +#endif int last_sync_time = so->last_sync_time [ssn]; int last_sync_length = so->last_sync_length[ssn]; int32_t delta = le->timestamp - last_sync_time; //Handle time wrapping (be sure to be int32) @@ -498,16 +502,29 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) if( le->length > so->pulselength_min_sync ) //Pulse longer indicates a sync pulse. { int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length; - + //printf( "INP: %d %d\n", is_new_pulse, so->sync_set_number ); so->did_handle_ootx = 0; if( is_new_pulse ) { int is_master_sync_pulse = delta > so->pulse_in_clear_time /*40000*/; + int is_pulse_from_same_lh_as_last_sweep; + int tp = delta % ( so->timecenter_ticks * 2); + is_pulse_from_same_lh_as_last_sweep = tp < so->pulse_synctime_slack && tp > -so->pulse_synctime_slack; - if( is_master_sync_pulse ) + if( is_master_sync_pulse ) //Could also be called by slave if no master was seen. { - ssn = so->sync_set_number = 0; + ssn = so->sync_set_number = is_pulse_from_same_lh_as_last_sweep?(so->sync_set_number):0; //If repeated lighthouse, just back off one. + if( ssn < 0 ) { SV_INFO( "SEVERE WARNING: Pulse codes for tracking not able to be backed out.\n" ); ssn = 0; } + if( ssn != 0 ) + { + //If it's the slave that is repeated, be sure to zero out its sync info. + so->last_sync_length[0] = 0; + } + else + { + so->last_sync_length[1] = 0; + } so->last_sync_time[ssn] = le->timestamp; so->last_sync_length[ssn] = le->length; } @@ -544,15 +561,19 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) } } - //Extra tidbit for storing length-of-sync-pulses. +#if 0 + //Extra tidbit for storing length-of-sync-pulses, if you want to try to use this to determine AoI or distance to LH. { int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz. int base_station = is_new_pulse; - //printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); - ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length, base_station); + printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); //XXX sync_set_number is wrong here. + ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length, base_station); //XXX sync_set_number is wrong here. } +#endif } + //Any else- statements below here are + //See if this is a valid actual pulse. else if( le->length < so->pulse_max_for_sweep && delta > so->pulse_in_clear_time && ssn >= 0 ) { @@ -576,9 +597,11 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) decode_acode(so->last_sync_length[1],main_divisor) }; + //printf( "%d %d\n", acode_array[0], acode_array[1] ); + //XXX: TODO: Capture error count here. - if( acode_array[0] < 0 ) return; - if( acode_array[1] < 0 ) return; + //if( acode_array[0] < 0 ) return; + //if( acode_array[1] < 0 ) return; int acode = acode_array[0]; @@ -587,13 +610,14 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) int32_t delta1 = so->last_sync_time[0] - so->recent_sync_time; int32_t delta2 = so->last_sync_time[1] - so->last_sync_time[0]; - ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0 ); - ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1 ); + //printf( "%p %p %d %d %d %p\n", ctx, so, so->last_sync_time[0], acode_array, so->last_sync_length[0], ctx->lightproc ); + if( acode_array[0] >= 0 ) ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0 ); + if( acode_array[1] >= 0 ) ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1 ); so->recent_sync_time = so->last_sync_time[1]; //Throw out everything if our sync pulses look like they're bad. - +/* int32_t center_1 = so->timecenter_ticks*2 - so->pulse_synctime_offset; int32_t center_2 = so->pulse_synctime_offset; int32_t slack = so->pulse_synctime_slack; @@ -611,7 +635,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) so->sync_set_number = -1; return; } - +*/ so->did_handle_ootx = 1; } @@ -628,7 +652,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) int32_t offset_from = le->timestamp - dl + le->length/2; //Make sure pulse is in valid window - if( offset_from < 380000 && offset_from > 70000 ) + if( offset_from < so->timecenter_ticks*2-so->pulse_in_clear_time && offset_from > so->pulse_in_clear_time && acode >= 0 ) { ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, !(acode>>2) ); } @@ -639,7 +663,6 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) //Runt pulse, or no sync pulses available. } #endif - } -- cgit v1.2.3 From 947dfaff73c0b758a21a410776543340e732d705 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 30 Apr 2017 00:46:57 -0400 Subject: Fix charles' disambiguator to work with one lighthouse and @axlecrusher's OOTX reader. --- src/survive_data.c | 125 ++++++++++++++++++++++++++++------------------------- test.c | 0 2 files changed, 65 insertions(+), 60 deletions(-) mode change 100755 => 100644 test.c diff --git a/src/survive_data.c b/src/survive_data.c index 0e2e828..4f94106 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -432,13 +432,59 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * le ) int32_t decode_acode(uint32_t length, int32_t main_divisor) { //+50 adds a small offset and seems to help always get it right. //Check the +50 in the future to see how well this works on a variety of hardware. - + if( !main_divisor ) return -1; int32_t acode = (length+main_divisor+50)/(main_divisor*2); if( acode & 1 ) return -1; return (acode>>1) - 6; } + +void HandleOOTX( SurviveContext * ctx, SurviveObject * so ) +{ + int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz. + + int32_t acode_array[2] = + { + decode_acode(so->last_sync_length[0],main_divisor), + decode_acode(so->last_sync_length[1],main_divisor) + }; + + + int32_t delta1 = so->last_sync_time[0] - so->recent_sync_time; + int32_t delta2 = so->last_sync_time[1] - so->last_sync_time[0]; + + //printf( "%p %p %d %d %d %p\n", ctx, so, so->last_sync_time[0], acode_array, so->last_sync_length[0], ctx->lightproc ); + if( acode_array[0] >= 0 ) ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0 ); + if( acode_array[1] >= 0 ) ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1 ); + + so->recent_sync_time = so->last_sync_time[1]; + +/* + //Throw out everything if our sync pulses look like they're bad. + //This actually doesn't seem to hold anymore, now that we're looking for multiple LHs. + int32_t center_1 = so->timecenter_ticks*2 - so->pulse_synctime_offset; + int32_t center_2 = so->pulse_synctime_offset; + int32_t slack = so->pulse_synctime_slack; + + if( delta1 < center_1 - slack || delta1 > center_1 + slack ) + { + //XXX: TODO: Count faults. + so->sync_set_number = -1; + return; + } + + if( delta2 < center_2 - slack || delta2 > center_2 + slack ) + { + //XXX: TODO: Count faults. + so->sync_set_number = -1; + return; + } +*/ + so->did_handle_ootx = 1; +} + + //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. void handle_lightcap( SurviveObject * so, LightcapElement * le ) { @@ -502,9 +548,18 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) if( le->length > so->pulselength_min_sync ) //Pulse longer indicates a sync pulse. { int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length; - //printf( "INP: %d %d\n", is_new_pulse, so->sync_set_number ); + + //TRICKY: If we didn't see anything from the other lighthouse, we might just not see it... But, we still have to send our sync + //information to the rest of libsurvive. This could be turned into a function and combined with the code below. + if( !so->did_handle_ootx && is_new_pulse ) + { + HandleOOTX( ctx, so ); + } so->did_handle_ootx = 0; + + //printf( "INP: %d %d\n", is_new_pulse, so->sync_set_number ); + if( is_new_pulse ) { int is_master_sync_pulse = delta > so->pulse_in_clear_time /*40000*/; @@ -563,6 +618,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) #if 0 //Extra tidbit for storing length-of-sync-pulses, if you want to try to use this to determine AoI or distance to LH. + //We don't actually use this anywhere, and I doubt we ever will? Though, it could be useful at a later time to improve tracking. { int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz. int base_station = is_new_pulse; @@ -590,71 +646,20 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) //Make sure it fits nicely into a divisible-by-500 time. int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz. - - int32_t acode_array[2] = - { - decode_acode(so->last_sync_length[0],main_divisor), - decode_acode(so->last_sync_length[1],main_divisor) - }; - - //printf( "%d %d\n", acode_array[0], acode_array[1] ); - - //XXX: TODO: Capture error count here. - //if( acode_array[0] < 0 ) return; - //if( acode_array[1] < 0 ) return; - - int acode = acode_array[0]; + int acode = decode_acode(so->last_sync_length[0],main_divisor); if( !so->did_handle_ootx ) - { - int32_t delta1 = so->last_sync_time[0] - so->recent_sync_time; - int32_t delta2 = so->last_sync_time[1] - so->last_sync_time[0]; - - //printf( "%p %p %d %d %d %p\n", ctx, so, so->last_sync_time[0], acode_array, so->last_sync_length[0], ctx->lightproc ); - if( acode_array[0] >= 0 ) ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0 ); - if( acode_array[1] >= 0 ) ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1 ); - - so->recent_sync_time = so->last_sync_time[1]; - - //Throw out everything if our sync pulses look like they're bad. -/* - int32_t center_1 = so->timecenter_ticks*2 - so->pulse_synctime_offset; - int32_t center_2 = so->pulse_synctime_offset; - int32_t slack = so->pulse_synctime_slack; - - if( delta1 < center_1 - slack || delta1 > center_1 + slack ) - { - //XXX: TODO: Count faults. - so->sync_set_number = -1; - return; - } - - if( delta2 < center_2 - slack || delta2 > center_2 + slack ) - { - //XXX: TODO: Count faults. - so->sync_set_number = -1; - return; - } -*/ - so->did_handle_ootx = 1; - } - - if (acode > 3) { - if( ssn == 0 ) - { - //SV_INFO( "Warning: got a slave marker but only got a master sync." ); - //This happens too frequently. Consider further examination. - } - dl = so->last_sync_time[1]; - tpco = so->last_sync_length[1]; - } + HandleOOTX( ctx, so ); int32_t offset_from = le->timestamp - dl + le->length/2; //Make sure pulse is in valid window - if( offset_from < so->timecenter_ticks*2-so->pulse_in_clear_time && offset_from > so->pulse_in_clear_time && acode >= 0 ) + if( offset_from < so->timecenter_ticks*2-so->pulse_in_clear_time && offset_from > so->pulse_in_clear_time ) { - ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, !(acode>>2) ); + int whichlh; + if( acode < 0 ) whichlh = 1; + else whichlh = !(acode>>2); + ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, whichlh ); } } else diff --git a/test.c b/test.c old mode 100755 new mode 100644 -- cgit v1.2.3 From ba11bb2755dd1dddf6f82893b897513d2e387239 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Sun, 30 Apr 2017 10:37:45 -0700 Subject: Fix regression that broke support for wireless controllers on Windows --- src/survive_vive.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/survive_vive.c b/src/survive_vive.c index 3ce985d..8f2a42e 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -344,7 +344,8 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * if (cur_dev->vendor_id == vendor_id && cur_dev->product_id == product_id) { - if( cur_dev->interface_number == enumid ) + if( cur_dev->interface_number == enumid || + cur_dev->interface_number == -1 && menum == enumid) { path_to_open = cur_dev->path; break; -- cgit v1.2.3 From 3560f3c7e397c0dd2bcadaeb028b8a9fdcb17096 Mon Sep 17 00:00:00 2001 From: Michael Turvey Date: Sun, 30 Apr 2017 22:22:55 -0700 Subject: Reduce Poser Calls I've been seeing starvation that manifests itself as weird light data coming into the disambiguator. A major cause ended up being starvation. This change will cut the frequency of poser calls by by 75% --- src/poser_turveytori.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index c526161..7abf5d0 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1520,7 +1520,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) counter++; // let's just do this occasionally for now... - //if (counter % 1 == 0) + if (counter % 4 == 0) QuickPose(so, 0); } // axis changed, time to increment the circular buffer index. -- cgit v1.2.3 From 72669fd63a9dc68ffe52887bb24ed7baef9975bf Mon Sep 17 00:00:00 2001 From: mwturvey Date: Tue, 2 May 2017 14:44:09 -0700 Subject: Fixing quatfrom2vectors --- redist/linmath.c | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/redist/linmath.c b/redist/linmath.c index caff1de..5fefe1e 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -520,17 +520,15 @@ void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest) FLT invs = 1 / s; FLT c[3]; - //cross3d(c, v0, v1); - cross3d(c, v1, v0); + cross3d(c, v0, v1); - q[0] = c[0] * invs; - q[1] = c[1] * invs; - q[2] = c[2] * invs; - q[3] = s * 0.5f; + q[0] = s * 0.5f; + q[1] = c[0] * invs; + q[2] = c[1] * invs; + q[3] = c[2] * invs; quatnormalize(q, q); } - } void matrix44copy(FLT * mout, const FLT * minm ) -- cgit v1.2.3 From d53ef13d5f8154d225c9f763131129d179db2441 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Fri, 5 May 2017 15:50:41 -0400 Subject: calibration values from MPU-6500 datasheet. I'm pretty sure the accelerometer values are correct. The gyroscope values may not be correct. --- src/survive_vive.c | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/survive_vive.c b/src/survive_vive.c index 030db8a..88b9fee 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1127,6 +1127,17 @@ void survive_data_cb( SurviveUSBInterface * si ) acceldata[3], acceldata[4], acceldata[5], 0,0,0 }; + //1G for accelerometer, from MPU6500 datasheet + //this can change if the firmware changes the sensitivity. + agm[0]/=8192.0f; + agm[1]/=8192.0f; + agm[2]/=8192.0f; + + //1000 deg/s for gyroscope, from MPU6500 datasheet + agm[3]/=65.5f; + agm[4]/=65.5f; + agm[5]/=65.5f; + ctx->imuproc( obj, 3, agm, timecode, code ); } } -- cgit v1.2.3 From 730bbd51f9aa0f0c6b57e9607d2b682ea1bf5ad2 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Fri, 5 May 2017 15:52:26 -0400 Subject: fix comment --- src/survive_vive.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/survive_vive.c b/src/survive_vive.c index 88b9fee..1f6a975 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1133,6 +1133,7 @@ void survive_data_cb( SurviveUSBInterface * si ) agm[1]/=8192.0f; agm[2]/=8192.0f; + //65.5 deg/s for gyroscope, from MPU6500 datasheet //1000 deg/s for gyroscope, from MPU6500 datasheet agm[3]/=65.5f; agm[4]/=65.5f; -- cgit v1.2.3 From 92d291674e5a12c7eedd314ffc4cf955d575cbea Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sat, 6 May 2017 22:05:32 -0400 Subject: add function to parse array of floats from json --- redist/json_helpers.c | 29 +++++++++++++++++++++++++++++ redist/json_helpers.h | 4 ++++ 2 files changed, 33 insertions(+) diff --git a/redist/json_helpers.c b/redist/json_helpers.c index c52301d..af7ddda 100644 --- a/redist/json_helpers.c +++ b/redist/json_helpers.c @@ -216,3 +216,32 @@ void json_load_file(const char* path) { free(JSON_STRING); } +int parse_float_array(char* str, jsmntok_t* token, FLT** f, uint8_t count) { + uint16_t i = 0; + + if (count==0) return 0; + + if (*f!=NULL) free(*f); + *f = malloc(sizeof(FLT) * count); + + for(i=0;iend; + char* s = str+token->start; + + #ifdef USE_DOUBLE + (*f)[i] = strtod(s, &end); + #else + (*f)[i] = strtof(s, &end); + #endif + + if (s == end) { + free(*f); + *f=NULL; + return 0; //not a float + } + token++; + } + + + return count; +} \ No newline at end of file diff --git a/redist/json_helpers.h b/redist/json_helpers.h index 1cccfe3..3ebf66b 100644 --- a/redist/json_helpers.h +++ b/redist/json_helpers.h @@ -4,6 +4,8 @@ #define JSON_HELPERS_H #include +#include +#include "survive_types.h" void json_write_float_array(FILE* f, const char* tag, float* v, uint8_t count); void json_write_double_array(FILE* f, const char* tag, double* v, uint8_t count); @@ -11,6 +13,8 @@ void json_write_uint32(FILE* f, const char* tag, uint32_t v); void json_write_float(FILE* f, const char* tag, float v); void json_write_str(FILE* f, const char* tag, const char* v); +int parse_float_array(char* str, jsmntok_t* token, FLT** values, uint8_t count); + void json_load_file(const char* path); extern void (*json_begin_object)(char* tag); extern void (*json_end_object)(); -- cgit v1.2.3 From 6c748afc397f29cd461e54c09d8a3cb45272f4ad Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sat, 6 May 2017 22:16:12 -0400 Subject: use calibration numbers from device json files --- include/libsurvive/survive.h | 6 ++++ src/survive_vive.c | 77 ++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 83 insertions(+) diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index e85fe48..1532d62 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -62,6 +62,12 @@ struct SurviveObject uint32_t last_lighttime; //May be a 24- or 32- bit number depending on what device. + FLT* acc_bias; // size is FLT*3. contains x,y,z + FLT* acc_scale; // size is FLT*3. contains x,y,z + FLT* gyro_bias; // size is FLT*3. contains x,y,z + FLT* gyro_scale; // size is FLT*3. contains x,y,z + + //Debug int tsl; }; diff --git a/src/survive_vive.c b/src/survive_vive.c index 1f6a975..720df77 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -22,6 +22,8 @@ #include // for alloca #endif +#include "json_helpers.h" + #ifdef HIDAPI #if defined(WINDOWS) || defined(WIN32) || defined (_WIN32) #include @@ -1057,6 +1059,34 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) } } +void calibrate_acc(SurviveObject* so, FLT* agm) { + if (so->acc_bias != NULL) { + agm[0] -= so->acc_bias[0]; + agm[1] -= so->acc_bias[1]; + agm[2] -= so->acc_bias[2]; + } + + if (so->acc_scale != NULL) { + agm[0] *= so->acc_scale[0]; + agm[1] *= so->acc_scale[1]; + agm[2] *= so->acc_scale[2]; + } +} + +void calibrate_gyro(SurviveObject* so, FLT* agm) { + if (so->gyro_bias != NULL) { + agm[0] -= so->gyro_bias[0]; + agm[1] -= so->gyro_bias[1]; + agm[2] -= so->gyro_bias[2]; + } + + if (so->gyro_bias != NULL) { + agm[0] *= so->gyro_scale[0]; + agm[1] *= so->gyro_scale[1]; + agm[2] *= so->gyro_scale[2]; + } +} + void survive_data_cb( SurviveUSBInterface * si ) { @@ -1127,12 +1157,16 @@ void survive_data_cb( SurviveUSBInterface * si ) acceldata[3], acceldata[4], acceldata[5], 0,0,0 }; + calibrate_acc(obj, agm); + //1G for accelerometer, from MPU6500 datasheet //this can change if the firmware changes the sensitivity. agm[0]/=8192.0f; agm[1]/=8192.0f; agm[2]/=8192.0f; + calibrate_gyro(obj, agm+3); + //65.5 deg/s for gyroscope, from MPU6500 datasheet //1000 deg/s for gyroscope, from MPU6500 datasheet agm[3]/=65.5f; @@ -1322,6 +1356,37 @@ printf( "Loading config: %d\n", len ); break; } } + + + if (jsoneq(ct0conf, tk, "acc_bias") == 0) { + int32_t count = (tk+1)->size; + FLT* values = NULL; + if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { + so->acc_bias = values; + } + } + if (jsoneq(ct0conf, tk, "acc_scale") == 0) { + int32_t count = (tk+1)->size; + FLT* values = NULL; + if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { + so->acc_scale = values; + } + } + + if (jsoneq(ct0conf, tk, "gyro_bias") == 0) { + int32_t count = (tk+1)->size; + FLT* values = NULL; + if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { + so->gyro_bias = values; + } + } + if (jsoneq(ct0conf, tk, "gyro_scale") == 0) { + int32_t count = (tk+1)->size; + FLT* values = NULL; + if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { + so->gyro_scale = values; + } + } } } else @@ -1361,6 +1426,12 @@ int survive_vive_close( SurviveContext * ctx, void * driver ) return 0; } +void init_SurviveObject(SurviveObject* so) { + so->acc_scale = NULL; + so->acc_bias = NULL; + so->gyro_scale = NULL; + so->gyro_bias = NULL; +} int DriverRegHTCVive( SurviveContext * ctx ) { @@ -1372,6 +1443,12 @@ int DriverRegHTCVive( SurviveContext * ctx ) SurviveObject * ww0 = calloc( 1, sizeof( SurviveObject ) ); SurviveViveData * sv = calloc( 1, sizeof( SurviveViveData ) ); + init_SurviveObject(hmd); + init_SurviveObject(wm0); + init_SurviveObject(wm1); + init_SurviveObject(tr0); + init_SurviveObject(ww0); + sv->ctx = ctx; #ifdef _WIN32 -- cgit v1.2.3 From a93ea0625daa2024ca20e04f1aa6da1d0ecf018c Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sat, 6 May 2017 22:19:14 -0400 Subject: check proper pointer for null --- src/survive_vive.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/survive_vive.c b/src/survive_vive.c index 720df77..95e688e 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1080,7 +1080,7 @@ void calibrate_gyro(SurviveObject* so, FLT* agm) { agm[2] -= so->gyro_bias[2]; } - if (so->gyro_bias != NULL) { + if (so->gyro_scale != NULL) { agm[0] *= so->gyro_scale[0]; agm[1] *= so->gyro_scale[1]; agm[2] *= so->gyro_scale[2]; -- cgit v1.2.3 From 85eaaff4d295771d28e11e80ae7f27d60eef0ed6 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 6 May 2017 23:54:04 -0400 Subject: Tweak calibration values. --- src/survive_vive.c | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/src/survive_vive.c b/src/survive_vive.c index 95e688e..c29465e 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1085,6 +1085,7 @@ void calibrate_gyro(SurviveObject* so, FLT* agm) { agm[1] *= so->gyro_scale[1]; agm[2] *= so->gyro_scale[2]; } + } @@ -1157,21 +1158,22 @@ void survive_data_cb( SurviveUSBInterface * si ) acceldata[3], acceldata[4], acceldata[5], 0,0,0 }; + agm[0]*=(float)(1./8192.0); + agm[1]*=(float)(1./8192.0); + agm[2]*=(float)(1./8192.0); calibrate_acc(obj, agm); //1G for accelerometer, from MPU6500 datasheet //this can change if the firmware changes the sensitivity. - agm[0]/=8192.0f; - agm[1]/=8192.0f; - agm[2]/=8192.0f; + + agm[3]*=(float)((1./32.768)*(3.14159/180.)); + agm[4]*=(float)((1./32.768)*(3.14159/180.)); + agm[5]*=(float)((1./32.768)*(3.14159/180.)); calibrate_gyro(obj, agm+3); //65.5 deg/s for gyroscope, from MPU6500 datasheet //1000 deg/s for gyroscope, from MPU6500 datasheet - agm[3]/=65.5f; - agm[4]/=65.5f; - agm[5]/=65.5f; ctx->imuproc( obj, 3, agm, timecode, code ); } @@ -1363,6 +1365,9 @@ printf( "Loading config: %d\n", len ); FLT* values = NULL; if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { so->acc_bias = values; + so->acc_bias[0] *= .125; //XXX Wat? Observed by CNL. Biasing by more than this seems to hose things. + so->acc_bias[1] *= .125; + so->acc_bias[2] *= .125; } } if (jsoneq(ct0conf, tk, "acc_scale") == 0) { -- cgit v1.2.3 From d8fe40d838badac49a013b2d1dc8c330411052c2 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sun, 7 May 2017 09:04:43 -0400 Subject: handle applying calibration numbers to watchman devices --- src/survive_vive.c | 73 ++++++++++++++++++++++++++++++++---------------------- 1 file changed, 44 insertions(+), 29 deletions(-) diff --git a/src/survive_vive.c b/src/survive_vive.c index 95e688e..6a685eb 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -814,6 +814,35 @@ int survive_get_config( char ** config, SurviveViveData * sv, int devno, int ifa #define POP2 (*(((uint16_t*)((readdata+=2)-2)))) #define POP4 (*(((uint32_t*)((readdata+=4)-4)))) +void calibrate_acc(SurviveObject* so, FLT* agm) { + if (so->acc_bias != NULL) { + agm[0] -= so->acc_bias[0]; + agm[1] -= so->acc_bias[1]; + agm[2] -= so->acc_bias[2]; + } + + if (so->acc_scale != NULL) { + agm[0] *= so->acc_scale[0]; + agm[1] *= so->acc_scale[1]; + agm[2] *= so->acc_scale[2]; + } +} + +void calibrate_gyro(SurviveObject* so, FLT* agm) { + if (so->gyro_bias != NULL) { + agm[0] -= so->gyro_bias[0]; + agm[1] -= so->gyro_bias[1]; + agm[2] -= so->gyro_bias[2]; + } + + if (so->gyro_scale != NULL) { + agm[0] *= so->gyro_scale[0]; + agm[1] *= so->gyro_scale[1]; + agm[2] *= so->gyro_scale[2]; + } +} + + static void handle_watchman( SurviveObject * w, uint8_t * readdata ) { @@ -891,6 +920,21 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) readdata[4], readdata[5], readdata[6], 0,0,0 }; +// if (w->acc_scale) printf("%f %f %f\n",w->acc_scale[0],w->acc_scale[1],w->acc_scale[2]); + calibrate_acc(w, agm); + + //I don't understand where these numbers come from but the data from the WMD seems to max out at 255... + agm[0]/=255.0f; + agm[1]/=255.0f; + agm[2]/=255.0f; + + calibrate_gyro(w, agm+3); + + //I don't understand where these numbers come from but the data from the WMD seems to max out at 255... + agm[3]/=255.0f; + agm[4]/=255.0f; + agm[5]/=255.0f; + w->ctx->imuproc( w, 3, agm, (time1<<24)|(time2<<16)|readdata[0], 0 ); int16_t * k = (int16_t *)readdata+1; @@ -1059,35 +1103,6 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) } } -void calibrate_acc(SurviveObject* so, FLT* agm) { - if (so->acc_bias != NULL) { - agm[0] -= so->acc_bias[0]; - agm[1] -= so->acc_bias[1]; - agm[2] -= so->acc_bias[2]; - } - - if (so->acc_scale != NULL) { - agm[0] *= so->acc_scale[0]; - agm[1] *= so->acc_scale[1]; - agm[2] *= so->acc_scale[2]; - } -} - -void calibrate_gyro(SurviveObject* so, FLT* agm) { - if (so->gyro_bias != NULL) { - agm[0] -= so->gyro_bias[0]; - agm[1] -= so->gyro_bias[1]; - agm[2] -= so->gyro_bias[2]; - } - - if (so->gyro_scale != NULL) { - agm[0] *= so->gyro_scale[0]; - agm[1] *= so->gyro_scale[1]; - agm[2] *= so->gyro_scale[2]; - } -} - - void survive_data_cb( SurviveUSBInterface * si ) { int size = si->actual_len; -- cgit v1.2.3 From 746a1eb0bf8df3effa55e79ff3513d9dcd9ac87c Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sun, 7 May 2017 09:14:07 -0400 Subject: use multiplication --- src/survive_vive.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/survive_vive.c b/src/survive_vive.c index b44e4fc..9a3cb03 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -924,16 +924,16 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) calibrate_acc(w, agm); //I don't understand where these numbers come from but the data from the WMD seems to max out at 255... - agm[0]/=255.0f; - agm[1]/=255.0f; - agm[2]/=255.0f; + agm[0]*=(1.0f/255.0f); + agm[1]*=(1.0f/255.0f); + agm[2]*=(1.0f/255.0f); calibrate_gyro(w, agm+3); //I don't understand where these numbers come from but the data from the WMD seems to max out at 255... - agm[3]/=255.0f; - agm[4]/=255.0f; - agm[5]/=255.0f; + agm[3]*=(1.0f/255.0f); + agm[4]*=(1.0f/255.0f); + agm[5]*=(1.0f/255.0f); w->ctx->imuproc( w, 3, agm, (time1<<24)|(time2<<16)|readdata[0], 0 ); -- cgit v1.2.3 From 63cb143f11ae493e36a1e73b672da13cc744532e Mon Sep 17 00:00:00 2001 From: CNLohr Date: Fri, 12 May 2017 10:22:42 -0400 Subject: Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 130fa1e..001881c 100644 --- a/README.md +++ b/README.md @@ -129,5 +129,5 @@ To support the Vive on HDMI, you either need a newer version of HDMI, or you nee ## Addendum and notes -Thanks to Mr. Fault for our logo! +Thanks to Mr. Faul for our logo! Special thanks to @nairol for an extreme amount of detail in reverse engineering the existing HTC Vive system on his https://github.com/nairol/LighthouseRedox project. -- cgit v1.2.3 From 78ac5da64c299bcf0f2e1c8d19c876e7723faccf Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 13 May 2017 03:47:35 -0400 Subject: Fix my disambiguator. Seems to work with either-or lighthouse. --- src/survive_data.c | 46 ++++++++++++++++------------------------------ 1 file changed, 16 insertions(+), 30 deletions(-) diff --git a/src/survive_data.c b/src/survive_data.c index df8df8e..157650d 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -468,6 +468,7 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * le ) #endif + int32_t decode_acode(uint32_t length, int32_t main_divisor) { //+50 adds a small offset and seems to help always get it right. //Check the +50 in the future to see how well this works on a variety of hardware. @@ -478,6 +479,10 @@ int32_t decode_acode(uint32_t length, int32_t main_divisor) { return (acode>>1) - 6; } +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +///////The charles disambiguator. Don't use this, mostly here for debugging./////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + void HandleOOTX( SurviveContext * ctx, SurviveObject * so ) { @@ -499,30 +504,9 @@ void HandleOOTX( SurviveContext * ctx, SurviveObject * so ) so->recent_sync_time = so->last_sync_time[1]; -/* - //Throw out everything if our sync pulses look like they're bad. - //This actually doesn't seem to hold anymore, now that we're looking for multiple LHs. - int32_t center_1 = so->timecenter_ticks*2 - so->pulse_synctime_offset; - int32_t center_2 = so->pulse_synctime_offset; - int32_t slack = so->pulse_synctime_slack; - - if( delta1 < center_1 - slack || delta1 > center_1 + slack ) - { - //XXX: TODO: Count faults. - so->sync_set_number = -1; - return; - } - - if( delta2 < center_2 - slack || delta2 > center_2 + slack ) - { - //XXX: TODO: Count faults. - so->sync_set_number = -1; - return; - } -*/ so->did_handle_ootx = 1; } - + //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. void handle_lightcap( SurviveObject * so, LightcapElement * le ) @@ -588,16 +572,8 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) { int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length; - //TRICKY: If we didn't see anything from the other lighthouse, we might just not see it... But, we still have to send our sync - //information to the rest of libsurvive. This could be turned into a function and combined with the code below. - if( !so->did_handle_ootx && is_new_pulse ) - { - HandleOOTX( ctx, so ); - } - so->did_handle_ootx = 0; - //printf( "INP: %d %d\n", is_new_pulse, so->sync_set_number ); if( is_new_pulse ) { @@ -606,6 +582,16 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) int tp = delta % ( so->timecenter_ticks * 2); is_pulse_from_same_lh_as_last_sweep = tp < so->pulse_synctime_slack && tp > -so->pulse_synctime_slack; + if( !so->did_handle_ootx ) + { + HandleOOTX( ctx, so ); + } + if( !is_master_sync_pulse ) + { + so->did_handle_ootx = 0; + } + + if( is_master_sync_pulse ) //Could also be called by slave if no master was seen. { ssn = so->sync_set_number = is_pulse_from_same_lh_as_last_sweep?(so->sync_set_number):0; //If repeated lighthouse, just back off one. -- cgit v1.2.3 From 4fda21d52d8b99b037a096d231848fb5588bee22 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 13 May 2017 14:09:17 -0400 Subject: remove last reference to non-cnfg graphics. --- redist/CNFGNullDriver.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/redist/CNFGNullDriver.c b/redist/CNFGNullDriver.c index 34346cc..c35884a 100644 --- a/redist/CNFGNullDriver.c +++ b/redist/CNFGNullDriver.c @@ -1,6 +1,6 @@ //Copyright (c) 2017 <>< Charles Lohr - Under the MIT/x11 or NewBSD License you choose. -#include "DrawFunctions.h" +#include "CNFGFunctions.h" static int w, h; void CNFGGetDimensions( short * x, short * y ) -- cgit v1.2.3 From 0e62c9035f52cbc0c0007aa8d29734d855d0bfa3 Mon Sep 17 00:00:00 2001 From: Stubenhocker1399 Date: Mon, 15 May 2017 00:40:07 +0200 Subject: Update README.md --- README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/README.md b/README.md index 001881c..5b25bb6 100644 --- a/README.md +++ b/README.md @@ -17,6 +17,10 @@ Most of the development is discussed on Discord. Join the chat and discussion h | Fifth livestream | https://www.youtube.com/watch?v=hHt3twW5_fI | 3:13:38 | | Sixth livestream | https://www.youtube.com/watch?v=JsfkNRFkFM4 | 3:44:49 | | Seventh livestream | https://www.youtube.com/watch?v=EKSHvO3QSWY | 1:17:21 | +| Eighth livestream | https://www.youtube.com/watch?v=nSbEltdH9vM | 6:06:36 | +| Ninth livestream | https://www.youtube.com/watch?v=60sGTd8T-KY | 3:28:44 | +| Ninth B livestream | https://www.youtube.com/watch?v=IIYj1Ig_gz8 | 2:25:33 | +| Tenth livestream | https://www.youtube.com/watch?v=boXRdXca6Qc | ?:??:?? | Notes from second livestream trying to reverse engineer the watchman protocol: https://gist.github.com/cnlohr/581c433f36f4249f8bbc9c2b6450ef0e -- cgit v1.2.3