1 files changed, 111 insertions, 91 deletions

diff --git a/proto-ft/ftnoir_protocol_ft.cpp b/proto-ft/ftnoir_protocol_ft.cpp
index 0c97ffd2..7f710621 100644
--- a/proto-ft/ftnoir_protocol_ft.cpp
+++ b/proto-ft/ftnoir_protocol_ft.cpp
@@ -1,4 +1,4 @@
-/* Copyright (c) 2013-2018 Stanislaw Halik <sthalik@misaki.pl>
+/* Copyright (c) 2013-2015, 2017 Stanislaw Halik <sthalik@misaki.pl>
  * Copyright (c) 2015 Wim Vriend
  *
  * Permission to use, copy, modify, and/or distribute this software for any
@@ -6,27 +6,39 @@
  * copyright notice and this permission notice appear in all copies.
  */
 
+#include "compat/library-path.hpp"
+
 #include "ftnoir_protocol_ft.h"
 #include "csv/csv.h"
-#include "compat/library-path.hpp"
+#include <QDir>
 
-#include <cassert>
 #include <cstddef>
-#include <atomic>
 #include <cmath>
-#include <cstdlib>
-
 #include <windows.h>
-#include <intrin.h>
 
-static int page_size()
+freetrack::~freetrack()
 {
-  SYSTEM_INFO system_info;
-  GetSystemInfo(&system_info);
-  return system_info.dwPageSize;
+    //dummyTrackIR.kill(); dummyTrackIR.waitForFinished();
+    if (s.ephemeral_library_location)
+    {
+        QSettings settings_ft("Freetrack", "FreetrackClient");
+        QSettings settings_npclient("NaturalPoint", "NATURALPOINT\\NPClient Location");
+
+        settings_ft.setValue("Path", "");
+        settings_npclient.setValue("Path", "");
+
+        if (dummyTrackIR.state() == dummyTrackIR.Running)
+        {
+            dummyTrackIR.kill();
+            dummyTrackIR.waitForFinished(100);
+        }
+    }
+    else
+        dummyTrackIR.close();
 }
 
-static_assert(sizeof(LONG) == 4u, "");
+static_assert(sizeof(LONG) == sizeof(std::int32_t));
+static_assert(sizeof(LONG) == 4u);
 
 never_inline void store(float volatile& place, const float value)
 {
@@ -34,58 +46,44 @@ never_inline void store(float volatile& place, const float value)
     {
         float f32;
         LONG i32;
-    } value_;
+    } value_ {};
 
     value_.f32 = value;
 
-    static_assert(sizeof(value_) == sizeof(float), "");
-    static_assert(offsetof(decltype(value_), f32) == offsetof(decltype(value_), i32), "");
+    static_assert(sizeof(value_) == sizeof(float));
+    static_assert(offsetof(decltype(value_), f32) == offsetof(decltype(value_), i32));
 
     (void)InterlockedExchange((LONG volatile*)&place, value_.i32);
 }
 
-template<typename t, typename u>
-force_inline
-std::enable_if_t<(std::is_integral_v<t>) && sizeof(t) == 4>
-store(t volatile& place, u value)
+template<typename t>
+static void store(t volatile& place, t value)
 {
-    (void)InterlockedExchange((LONG volatile*) &place, value);
+    static_assert(sizeof(t) == 4u);
+    (void)InterlockedExchange((LONG volatile*) &place, (LONG)value);
 }
 
-force_inline std::int32_t load(std::int32_t volatile& place)
+static std::int32_t load(std::int32_t volatile& place)
 {
     return InterlockedCompareExchange((volatile LONG*) &place, 0, 0);
 }
 
-freetrack::freetrack()
+void freetrack::pose(const double* headpose, const double* raw)
 {
-}
+    constexpr double d2r = M_PI/180;
 
-freetrack::~freetrack()
-{
-    if (shm.success())
-    {
-        store(pMemData->data.DataID, 0);
-        store(pMemData->GameID2, -1);
-    }
-
-    dummyTrackIR.close();
-}
-
-void freetrack::pose(const double* headpose)
-{
-    const float yaw = -degrees_to_rads(headpose[Yaw]);
-    const float roll = degrees_to_rads(headpose[Roll]);
+    const float yaw = float(-headpose[Yaw] * d2r);
+    const float roll = float(headpose[Roll] * d2r);
     const float tx = float(headpose[TX] * 10);
     const float ty = float(headpose[TY] * 10);
     const float tz = float(headpose[TZ] * 10);
 
     // HACK: Falcon BMS makes a "bump" if pitch is over the value -sh 20170615
-    const bool is_crossing_90 = std::fabs(headpose[Pitch] - 90) < 1e-4;
-    const float pitch = -degrees_to_rads(is_crossing_90 ? 89.85 : headpose[Pitch]);
+    const bool is_crossing_90 = std::fabs(headpose[Pitch] - 90) < .15;
+    const float pitch = float(-d2r * (is_crossing_90 ? 89.86 : headpose[Pitch]));
 
-    FTHeap* ft = pMemData;
-    FTData* data = &ft->data;
+    FTHeap* const ft = pMemData;
+    FTData* const data = &ft->data;
 
     store(data->X, tx);
     store(data->Y, ty);
@@ -95,12 +93,14 @@ void freetrack::pose(const double* headpose)
     store(data->Pitch, pitch);
     store(data->Roll, roll);
 
-    const std::int32_t id = load(ft->GameID);
-
-    data_id++;
-    data_id %= 128;
+    store(data->RawYaw, float(-raw[Yaw] * d2r));
+    store(data->RawPitch, float(raw[Pitch] * d2r));
+    store(data->RawRoll, float(raw[Roll] * d2r));
+    store(data->RawX, float(raw[TX] * 10));
+    store(data->RawY, float(raw[TY] * 10));
+    store(data->RawZ, float(raw[TZ] * 10));
 
-    store(data->DataID, 60 * 5 + data_id);
+    const std::int32_t id = load(ft->GameID);
 
     if (intGameID != id)
     {
@@ -108,45 +108,49 @@ void freetrack::pose(const double* headpose)
         union  {
             unsigned char table[8];
             std::int32_t ints[2];
-        } t;
+        } t {};
 
         t.ints[0] = 0; t.ints[1] = 0;
 
         (void)CSV::getGameData(id, t.table, gamename);
 
-        static_assert(sizeof(LONG) == 4, "");
-        static_assert(sizeof(int) == 4, "");
-
-        // memory mappings are page-aligned due to TLB
-        if ((std::intptr_t(pMemData) & page_size() - 1) != 0)
-            assert(!"proto/freetrack: memory mapping not page aligned");
+        {
+            // FTHeap pMemData happens to be aligned on a page boundary by virtue of
+            // memory mapping usage (MS Windows equivalent of mmap(2)).
+            static_assert((offsetof(FTHeap, table) & (sizeof(LONG)-1)) == 0);
 
-        // no atomic access for `char'
-        for (unsigned k = 0; k < 2; k++)
-            store(pMemData->table_ints[k], t.ints[k]);
+            for (unsigned k = 0; k < 2; k++)
+                store(pMemData->table_ints[k], t.ints[k]);
+        }
 
         store(ft->GameID2, id);
+        store(data->DataID, 0u);
 
         intGameID = id;
 
+        if (gamename.isEmpty())
+            gamename = tr("Unknown game");
+
         QMutexLocker foo(&game_name_mutex);
         connected_game = gamename;
     }
+    else
+        (void)InterlockedAdd((LONG volatile*)&data->DataID, 1);
 }
 
-float freetrack::degrees_to_rads(double degrees)
+QString freetrack::game_name()
 {
-    return float(degrees*M_PI/180);
+    QMutexLocker foo(&game_name_mutex);
+    return connected_game;
 }
 
-void freetrack::start_dummy()
-{
+void freetrack::start_dummy() {
     static const QString program = OPENTRACK_BASE_PATH + OPENTRACK_LIBRARY_PATH "TrackIR.exe";
     dummyTrackIR.setProgram("\"" + program + "\"");
     dummyTrackIR.start();
 }
 
-void freetrack::set_protocols(bool ft, bool npclient)
+module_status freetrack::set_protocols()
 {
     static const QString program_dir = OPENTRACK_BASE_PATH + OPENTRACK_LIBRARY_PATH;
 
@@ -154,44 +158,61 @@ void freetrack::set_protocols(bool ft, bool npclient)
     QSettings settings_ft("Freetrack", "FreetrackClient");
     QSettings settings_npclient("NaturalPoint", "NATURALPOINT\\NPClient Location");
 
-    if (ft)
-        settings_ft.setValue("Path", program_dir);
-    else
-        settings_ft.setValue("Path", "");
+    QString location = *s.custom_location_pathname;
+
+    bool use_freetrack = true, use_npclient = true;
+    switch (*s.used_interface)
+    {
+    case settings::enable_npclient: use_freetrack = false; break;
+    case settings::enable_freetrack: use_npclient = false; break;
+    case settings::enable_both: use_freetrack = true; use_npclient = true; break;
+    default:
+        return error(tr("proto/freetrack: wrong interface selection '%1'")
+               .arg(*s.used_interface));
+    }
 
-    if (npclient)
-        settings_npclient.setValue("Path", program_dir);
+    if (!s.use_custom_location || s.custom_location_pathname->isEmpty() || !QDir{s.custom_location_pathname}.exists())
+        location = program_dir;
     else
-        settings_npclient.setValue("Path", "");
+    {
+        bool copy = true;
+
+        if (use_npclient && !QFile{location + "/NPClient.dll"}.exists())
+            copy &= QFile::copy(program_dir + "/NPClient.dll", location + "/NPClient.dll");
+        if (use_npclient && !QFile{location + "/NPClient64.dll"}.exists())
+            copy &= QFile::copy(program_dir + "/NPClient64.dll", location + "/NPClient64.dll");
+        if (use_freetrack && !QFile{location + "/freetrackclient.dll"}.exists())
+            copy &= QFile::copy(program_dir + "/freetrackclient.dll", location + "/freetrackclient.dll");
+        if (use_freetrack && !QFile{location + "/freetrackclient64.dll"}.exists())
+            copy &= QFile::copy(program_dir + "/freetrackclient64.dll", location + "/freetrackclient64.dll");
+
+        if (!copy)
+            return {tr("Can't copy library to selected custom location '%1'").arg(s.custom_location_pathname)};
+    }
+
+    location.replace('\\', '/');
+    if (!location.endsWith('/'))
+        location += '/';
+
+    settings_ft.setValue("Path", use_freetrack ? location : "");
+    settings_npclient.setValue("Path", use_npclient ? location : "");
+
+    return {};
 }
 
 module_status freetrack::initialize()
 {
     if (!shm.success())
-        return error(_("Can't load freetrack memory mapping"));
-
-    bool use_ft = false, use_npclient = false;
-
-    switch (s.intUsedInterface) {
-    default:
-    case 0:
-        use_ft = true;
-        use_npclient = true;
-        break;
-    case 1:
-        use_ft = true;
-        break;
-    case 2:
-        use_npclient = true;
-        break;
-    }
+        return error(tr("Can't load freetrack memory mapping"));
 
-    set_protocols(use_ft, use_npclient);
+    if (auto ret = set_protocols(); !ret.is_ok())
+        return ret;
 
+    pMemData->data.DataID = 1;
     pMemData->data.CamWidth = 100;
     pMemData->data.CamHeight = 250;
 
-#if 1
+#if 0
     store(pMemData->data.X1, float(100));
     store(pMemData->data.Y1, float(200));
     store(pMemData->data.X2, float(300));
@@ -200,14 +221,13 @@ module_status freetrack::initialize()
     store(pMemData->data.Y3, float(100));
 #endif
 
-    store(pMemData->GameID2, -1);
-    store(pMemData->data.DataID, 0);
+    store(pMemData->GameID2, 0);
 
     for (unsigned k = 0; k < 2; k++)
         store(pMemData->table_ints[k], 0);
 
     // more games need the dummy executable than previously thought
-    if (use_npclient)
+    if (s.used_interface != settings::enable_freetrack)
         start_dummy();
 
     return status_ok();