From 0c4ee9855309f2bdadd777e642cbbe6d4a3c5e4a Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Sat, 27 Sep 2014 10:42:47 +0200
Subject: freepie: once more, with feeling (tm)

- don't hand-code octet reversal
- don't check for endianness every iteration
- in flag_Raw case float[6] orient was being clobbered, use a temporary
- reset float[6]* orient ptr to null after usage
- don't special-case .apk sending too short dgrams

Thanks-to: @KyokushinPL
---
 .../ftnoir_tracker_freepie-udp.cpp                 | 90 ++++++++++------------
 1 file changed, 42 insertions(+), 48 deletions(-)

diff --git a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp
index deea6c7c..26726b18 100644
--- a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp
+++ b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp
@@ -2,6 +2,7 @@
 #include "facetracknoir/plugin-support.h"
 
 #include <cinttypes>
+#include <algorithm>
 
 TrackerImpl::TrackerImpl() : pose { 0,0,0, 0,0,0 }, should_quit(false)
 {
@@ -32,62 +33,55 @@ void TrackerImpl::run() {
         Mask = flag_Raw | flag_Orient
     };
 
-    while (1) {
-        struct check {
-            union {
-                std::uint16_t half;
-                unsigned char bytes[2];
-            };
-            bool convertp;
-            check() : bytes { 0, 255 }, convertp(half > 255) {}
-        } crapola;
+    struct check {
+        union {
+            std::uint16_t half;
+            unsigned char bytes[2];
+        };
+        bool convertp;
+        check() : bytes { 0, 255 }, convertp(half > 255) {}
+    } crapola;
 
+    while (1) {
         if (should_quit)
             break;
-        {
-            float* orient = nullptr;
 
-            while (sock.hasPendingDatagrams())
-            {
-                data = decltype(data){0,0, {0,0,0, 0,0,0}};
-                int sz = sock.readDatagram(reinterpret_cast<char*>(&data), sizeof(data));
-
-                int flags = data.flags & F::Mask;
+        float* orient = nullptr;
 
-                using t = decltype(data);
-                static constexpr int minsz = offsetof(t, raw_rot) + sizeof(t::raw_rot);
-                const bool flags_came_out_wrong = minsz > sz;
-
-                if (flags_came_out_wrong)
-                    flags &= ~F::flag_Raw;
+        while (sock.hasPendingDatagrams())
+        {
+            using t = decltype(data);
+            t tmp {0,0, {0,0,0, 0,0,0}};
+            (void) sock.readDatagram(reinterpret_cast<char*>(&tmp), sizeof(data));
+            int flags = data.flags & F::Mask;
 
-                switch (flags)
-                {
-                case flag_Raw:
-                    continue;
-                case flag_Raw | flag_Orient:
-                    orient = data.raw_rot.rot;
-                    break;
-                case flag_Orient:
-                    orient = data.rot;
-                    break;
-                }
+            switch (flags)
+            {
+            case flag_Raw:
+            continue;
+            case flag_Raw | flag_Orient:
+                orient = data.raw_rot.rot;
+            break;
+            case flag_Orient:
+                orient = data.rot;
+            break;
             }
-            if (orient)
+            data = tmp;
+        }
+        if (orient)
+        {
+            if (crapola.convertp)
             {
-                if (crapola.convertp)
-                {
-                    constexpr int sz = sizeof(float[6]);
-                    const int len = sz / 2;
-                    unsigned char* alias = reinterpret_cast<unsigned char*>(orient);
-                    for (int i = 0; i < len; i++)
-                        alias[i] = alias[sz-i];
-                }
-
-                QMutexLocker foo(&mtx);
-                for (int i = 0; i < 3; i++)
-                    pose[Yaw + i] = orient[i];
+                unsigned char* alias = reinterpret_cast<unsigned char*>(orient);
+                constexpr int sz = sizeof(float[6]);
+                std::reverse(alias, alias + sz);
             }
+
+            QMutexLocker foo(&mtx);
+            for (int i = 0; i < 3; i++)
+                pose[Yaw + i] = orient[i];
+
+            orient = nullptr;
         }
         usleep(4000);
     }
@@ -96,7 +90,7 @@ void TrackerImpl::run() {
 void TrackerImpl::StartTracker(QFrame*)
 {
     (void) sock.bind(QHostAddress::Any, (int) s.port, QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint);
-  start();
+    start();
 }
 
 void TrackerImpl::GetHeadPoseData(double *data)
-- 
cgit v1.2.3