Add opentrack UDP headtracking support

UDP packets are handled with nanosockets
External headtracking state takes priority over look pitch/yaw axes (joystick-based headtracking).

src/ship/Headtracker.cpp

@@ -0,0 +1,99 @@
+// Copyright © 2008-2023 Pioneer Developers. See AUTHORS.txt for details
+// Licensed under the terms of the GPL v3. See licenses/GPL-3.txt
+
+#include "Headtracker.h"
+#include "core/Log.h"
+
+#include <nanosockets/nanosockets.h>
+#include <cstdint>
+
+HeadtrackingManager::HeadtrackingManager() :
+	m_trackerState {},
+	m_trackerSocket {},
+	m_connected(false)
+{ }
+
+HeadtrackingManager::~HeadtrackingManager()
+{
+	if (m_connected)
+		Disconnect();
+}
+
+bool HeadtrackingManager::Connect(const char *host, uint16_t port)
+{
+	if (nanosockets_initialize()) {
+		Log::Error("Headtracking: Error initializing socket library");
+		return false;
+	}
+
+	NanoAddress address = {};
+	address.port = port;
+
+	if (!host || std::strlen(host) == 0) {
+		if (nanosockets_address_set_ip(&address, "127.0.0.1")) {
+			Log::Error("Headtracking: error setting default address");
+			return false;
+		}
+	} else {
+		if (nanosockets_address_set_hostname(&address, host)) {
+			Log::Error("Headtracking: Error setting hostname to {}", host);
+			return false;
+		}
+	}
+
+	char ipaddr[64] = { 0 };
+	nanosockets_address_get_ip(&address, ipaddr, sizeof(ipaddr));
+	Log::Info("Listening for headtracking packets on {}:{}", ipaddr, port);
+
+	m_trackerSocket = nanosockets_create(1024, 1024);
+	if (m_trackerSocket == NANOSOCKETS_STATUS_ERROR) {
+		Log::Error("Headtracking: Error creating UDP socket");
+		return false;
+	}
+
+	if (nanosockets_bind(m_trackerSocket, &address)) {
+		Log::Error("Headtracking: Error binding to UDP socket {}:{}", ipaddr, port);
+
+		nanosockets_destroy(&m_trackerSocket);
+		return false;
+	}
+
+	m_connected = true;
+	return true;
+}
+
+void HeadtrackingManager::Disconnect()
+{
+	nanosockets_destroy(&m_trackerSocket);
+	nanosockets_deinitialize();
+
+	m_connected = false;
+}
+
+const HeadtrackingManager::State *HeadtrackingManager::GetHeadState() const
+{
+	return &m_trackerState;
+}
+
+void HeadtrackingManager::Update()
+{
+	if (!m_connected)
+		return;
+
+	int ready = nanosockets_poll(m_trackerSocket, 0);
+	if (!ready || ready == -1)
+		return;
+
+	uint8_t packet[sizeof(State)] = {};
+	memset(packet, 0, sizeof(State));
+
+	while (nanosockets_poll(m_trackerSocket, 0) == 1) {
+		NanoAddress _addr;
+		int bytesRecv = nanosockets_receive(m_trackerSocket, &_addr, packet, sizeof(State));
+
+		// Packet is the wrong size for headtracking, ignore it
+		if (bytesRecv == sizeof(State)) {
+			memcpy(&m_trackerState, packet, sizeof(State));
+		}
+	}
+}

src/ship/Headtracker.h

@@ -0,0 +1,36 @@
+// Copyright © 2008-2023 Pioneer Developers. See AUTHORS.txt for details
+// Licensed under the terms of the GPL v3. See licenses/GPL-3.txt
+
+#pragma once
+
+#include <cstdint>
+
+typedef int64_t NanoSocket;
+
+class HeadtrackingManager {
+public:
+	// opentrack UDP protocol
+	struct State {
+		// XYZ translation coordinates in meters
+		double x, y, z;
+		// yaw pitch roll in degrees
+		double yaw, pitch, roll;
+	};
+
+public:
+	HeadtrackingManager();
+	~HeadtrackingManager();
+
+	bool Connect(const char *host, uint16_t port);
+	void Disconnect();
+
+	const State *GetHeadState() const;
+
+	void Update();
+
+private:
+	State m_trackerState;
+	NanoSocket m_trackerSocket;
+
+	bool m_connected;
+};

src/ship/ShipViewController.cpp

@@ -4,7 +4,9 @@
 #include "ShipViewController.h"
 
 #include "CameraController.h"
+#include "GameConfig.h"
 #include "GameSaveError.h"
+#include "Headtracker.h"
 #include "Input.h"
 #include "WorldView.h"
 
@@ -73,6 +75,9 @@ ShipViewController::ShipViewController(WorldView *v) :
 	InputBindings.RegisterBindings();
 }
 
+ShipViewController::~ShipViewController()
+{}
+
 void ShipViewController::LoadFromJson(const Json &jsonObj)
 {
 	if (!jsonObj["cam_type"].is_number_integer())
@@ -103,6 +108,12 @@ void ShipViewController::Init()
 	m_siderealCameraController.reset(new SiderealCameraController(m_cameraContext, Pi::player));
 	m_flybyCameraController.reset(new FlyByCameraController(m_cameraContext, Pi::player));
 	SetCamType(m_camType); //set the active camera
+
+	std::string headtrackingIP = Pi::config->String("HeadtrackingIP", "");
+	int port = Pi::config->Int("HeadtrackingPort", 4242);
+
+	m_headtrackingManager.reset(new HeadtrackingManager());
+	m_headtrackingManager->Connect(headtrackingIP.c_str(), port);
 }
 
 void ShipViewController::Activated()
@@ -183,6 +194,8 @@ void ShipViewController::Update()
 	auto *cam = static_cast<MoveableCameraController *>(m_activeCameraController);
 	auto frameTime = Pi::GetFrameTime();
 
+	m_headtrackingManager->Update();
+
 	if (!InputBindings.active) {
 		m_activeCameraController->Update();
 
@@ -213,7 +226,16 @@ void ShipViewController::Update()
 			InputBindings.lookYaw->GetValue() * M_PI / 2.0,
 			0.0);
 
-		if (rotate.LengthSqr() > 0.0001) {
+		const HeadtrackingManager::State *headState = m_headtrackingManager->GetHeadState();
+		vector3f headRot = vector3f(
+			DEG2RAD(-headState->pitch),
+			DEG2RAD(-headState->yaw),
+			DEG2RAD(headState->roll));
+
+		if (headRot.LengthSqr() > 0.0001) {
+			cam->SetRotationAngles(headRot);
+			headtracker_input_priority = true;
+		} else if (rotate.LengthSqr() > 0.0001) {
 			cam->SetRotationAngles(rotate);
 			headtracker_input_priority = true;
 		} else if (headtracker_input_priority) {

src/ship/ShipViewController.h

@@ -9,9 +9,12 @@
 #include "ViewController.h"
 #include "utils.h"
 
+class HeadtrackingManager;
+
 class ShipViewController : public ViewController {
 public:
 	ShipViewController(WorldView *v);
+	~ShipViewController();
 
 	void Update() override;
 	void Activated() override;
@@ -44,6 +47,8 @@ class ShipViewController : public ViewController {
 
 	sigc::connection m_onMouseWheelCon;
 
+	std::unique_ptr<HeadtrackingManager> m_headtrackingManager;
+
 	std::unique_ptr<InternalCameraController> m_internalCameraController;
 	std::unique_ptr<ExternalCameraController> m_externalCameraController;
 	std::unique_ptr<SiderealCameraController> m_siderealCameraController;