diff options
author | Stanislaw Halik <sthalik@misaki.pl> | 2014-10-19 16:08:46 +0200 |
---|---|---|
committer | Stanislaw Halik <sthalik@misaki.pl> | 2014-10-19 18:28:08 +0200 |
commit | dd87370ff2818ee3c3d67ba104405b85ee44daba (patch) | |
tree | d5ebdf22aaccd0e345ce7a7c2ecb7b7193ea0791 /opentrack | |
parent | 7e2e341c45b86f76e08d74e056929ad76ff1a383 (diff) |
decruft more
Diffstat (limited to 'opentrack')
-rw-r--r-- | opentrack/export.hpp | 7 | ||||
-rw-r--r-- | opentrack/main-settings.hpp | 59 | ||||
-rw-r--r-- | opentrack/mappings.hpp | 88 | ||||
-rw-r--r-- | opentrack/options.hpp | 392 | ||||
-rw-r--r-- | opentrack/plugin-api.hpp | 11 | ||||
-rw-r--r-- | opentrack/plugin-qt-api.hpp | 63 | ||||
-rw-r--r-- | opentrack/plugin-support.cpp | 215 | ||||
-rw-r--r-- | opentrack/plugin-support.h | 51 | ||||
-rw-r--r-- | opentrack/pose.hpp | 44 | ||||
-rw-r--r-- | opentrack/qcopyable-mutex.hpp | 37 | ||||
-rw-r--r-- | opentrack/quat.hpp | 66 | ||||
-rw-r--r-- | opentrack/timer.hpp | 68 | ||||
-rw-r--r-- | opentrack/tracker.cpp | 187 | ||||
-rw-r--r-- | opentrack/tracker.h | 47 |
14 files changed, 1335 insertions, 0 deletions
diff --git a/opentrack/export.hpp b/opentrack/export.hpp new file mode 100644 index 00000000..8c8bdc69 --- /dev/null +++ b/opentrack/export.hpp @@ -0,0 +1,7 @@ +#pragma once +#ifdef _WIN32 +# define OPENTRACK_LINKAGE __declspec(dllexport) +#else +# define OPENTRACK_LINKAGE +#endif +#define OPENTRACK_EXPORT __attribute__ ((visibility ("default"))) OPENTRACK_LINKAGE diff --git a/opentrack/main-settings.hpp b/opentrack/main-settings.hpp new file mode 100644 index 00000000..6f78ac82 --- /dev/null +++ b/opentrack/main-settings.hpp @@ -0,0 +1,59 @@ +#pragma once + +#include <QString> +#include "opentrack/options.hpp" +using namespace options; + +struct key_opts { + value<int> key_index; + value<bool> ctrl, alt, shift; + key_opts(pbundle b, const QString& name) : + key_index(b, QString("key-index-%1").arg(name), 0), + ctrl(b, QString("key-ctrl-%1").arg(name), 0), + alt(b, QString("key-alt-%1").arg(name), 0), + shift(b, QString("key-shift-%1").arg(name), 0) + {} +}; + +struct axis_opts { + value<double> zero; + value<bool> invert, altp; + value<int> src; + axis_opts(pbundle b, QString pfx, int idx) : + zero(b, n(pfx, "zero-pos"), 0), + invert(b, n(pfx, "invert-axis"), false), + altp(b, n(pfx, "alt-axis-sign"), false), + src(b, n(pfx, "source-index"), idx) + {} +private: + static inline QString n(QString pfx, QString name) { + return QString("%1-%2").arg(pfx, name); + } +}; + +struct main_settings { + pbundle b; + key_opts center_key; + key_opts toggle_key; + value<QString> tracker_dll, tracker2_dll, filter_dll, protocol_dll; + axis_opts a_x, a_y, a_z, a_yaw, a_pitch, a_roll; + value<bool> tcomp_p, tcomp_tz, dingp; + main_settings(pbundle b) : + b(b), + center_key(b, "center"), + toggle_key(b, "toggle"), + tracker_dll(b, "tracker-dll", ""), + tracker2_dll(b, "tracker2-dll", ""), + filter_dll(b, "filter-dll", ""), + protocol_dll(b, "protocol-dll", ""), + a_x(b, "x", TX), + a_y(b, "y", TY), + a_z(b, "z", TZ), + a_yaw(b, "yaw", Yaw), + a_pitch(b, "pitch", Pitch), + a_roll(b, "roll", Roll), + tcomp_p(b, "compensate-translation", true), + tcomp_tz(b, "compensate-translation-disable-z-axis", false), + dingp(b, "ding", true) + {} +}; diff --git a/opentrack/mappings.hpp b/opentrack/mappings.hpp new file mode 100644 index 00000000..86126db9 --- /dev/null +++ b/opentrack/mappings.hpp @@ -0,0 +1,88 @@ +#pragma once + +#include <QSettings> +#include "options.hpp" +using namespace options; +#include "../qfunctionconfigurator/functionconfig.h" +#include "main-settings.hpp" + +class Mapping { +public: + Mapping(QString primary, + QString secondary, + int maxInput1, + int maxOutput1, + int maxInput2, + int maxOutput2, + axis_opts& opts) : + curve(maxInput1, maxOutput1), + curveAlt(maxInput2, maxOutput2), + opts(opts), + name1(primary), + name2(secondary) + { + // XXX TODO move all this qsettings boilerplate into a single header -sh 20141004 + QSettings settings("opentrack"); + QString currentFile = settings.value ( "SettingsFile", QCoreApplication::applicationDirPath() + "/settings/default.ini" ).toString(); + QSettings iniFile(currentFile, QSettings::IniFormat); + curve.loadSettings(iniFile, primary); + curveAlt.loadSettings(iniFile, secondary); + } + Map curve; + Map curveAlt; + axis_opts& opts; + QString name1, name2; +}; + +class Mappings { +private: + Mapping axes[6]; +public: + Mappings(std::vector<axis_opts*> opts) : + axes { + Mapping("tx","tx_alt", 100, 100, 100, 100, *opts[TX]), + Mapping("ty","ty_alt", 100, 100, 100, 100, *opts[TY]), + Mapping("tz","tz_alt", 100, 100, 100, 100, *opts[TZ]), + Mapping("rx", "rx_alt", 180, 180, 180, 180, *opts[Yaw]), + Mapping("ry", "ry_alt", 180, 180, 180, 180, *opts[Pitch]), + Mapping("rz", "rz_alt", 180, 180, 180, 180, *opts[Roll]) + } + {} + + inline Mapping& operator()(int i) { return axes[i]; } + inline const Mapping& operator()(int i) const { return axes[i]; } + + void load_mappings() + { + QSettings settings("opentrack"); + QString currentFile = settings.value ( "SettingsFile", QCoreApplication::applicationDirPath() + "/settings/default.ini" ).toString(); + QSettings iniFile( currentFile, QSettings::IniFormat ); + + for (int i = 0; i < 6; i++) + { + axes[i].curve.loadSettings(iniFile, axes[i].name1); + axes[i].curveAlt.loadSettings(iniFile, axes[i].name2); + } + } + void save_mappings() + { + QSettings settings("opentrack"); + QString currentFile = settings.value("SettingsFile", QCoreApplication::applicationDirPath() + "/settings/default.ini").toString(); + QSettings iniFile(currentFile, QSettings::IniFormat); + + for (int i = 0; i < 6; i++) + { + axes[i].curve.saveSettings(iniFile, axes[i].name1); + axes[i].curveAlt.saveSettings(iniFile, axes[i].name2); + } + } + + void invalidate_unsaved() + { + for (int i = 0; i < 6; i++) + { + axes[i].curve.invalidate_unsaved_settings(); + axes[i].curveAlt.invalidate_unsaved_settings(); + } + } +}; diff --git a/opentrack/options.hpp b/opentrack/options.hpp new file mode 100644 index 00000000..6c15d729 --- /dev/null +++ b/opentrack/options.hpp @@ -0,0 +1,392 @@ +/* Copyright (c) 2013-2014 Stanislaw Halik + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#pragma once + +#include <memory> +#include <tuple> +#include <map> +#include <string> + +#include <QObject> +#include <QSettings> +#include <QString> +#include <QVariant> +#include <QMutex> +#include <QMutexLocker> +#include <QWidget> +#include <QComboBox> +#include <QCheckBox> +#include <QDoubleSpinBox> +#include <QSpinBox> +#include <QSlider> +#include <QLineEdit> +#include <QLabel> +#include <QCoreApplication> + +#include <cinttypes> + +#include <QDebug> + +namespace options { + template<typename k, typename v> + using map = std::map<k, v>; + using std::string; + + template<typename t> + // don't elide usages of the function, qvariant default implicit + // conversion results in nonsensical runtime behavior -sh + inline t qcruft_to_t (const QVariant& datum); + + template<> + inline int qcruft_to_t<int>(const QVariant& t) + { + return t.toInt(); + } + + template<> + inline QString qcruft_to_t<QString>(const QVariant& t) + { + return t.toString(); + } + + template<> + inline bool qcruft_to_t<bool>(const QVariant& t) + { + return t.toBool(); + } + + template<> + inline double qcruft_to_t<double>(const QVariant& t) + { + return t.toDouble(); + } + + template<> + inline QVariant qcruft_to_t<QVariant>(const QVariant& t) + { + return t; + } + + // snapshot of qsettings group at given time + class group { + private: + map<string, QVariant> kvs; + string name; + static const QString ini_pathname() + { + QSettings settings(group::org); + return settings.value("SettingsFile", + QCoreApplication::applicationDirPath() + "/settings/default.ini" ).toString(); + } + public: + group(const string& name) : name(name) + { + QSettings conf(ini_pathname(), QSettings::IniFormat); + auto q_name = QString::fromStdString(name); + conf.beginGroup(q_name); + for (auto& k_ : conf.childKeys()) + { + auto tmp = k_.toUtf8(); + string k(tmp); + kvs[k] = conf.value(k_); + } + conf.endGroup(); + } + static constexpr const char* org = "opentrack"; + + void save() + { + QSettings s(ini_pathname(), QSettings::IniFormat); + auto q_name = QString::fromStdString(name); + s.beginGroup(q_name); + for (auto& i : kvs) + { + auto k = QString::fromStdString(i.first); + s.setValue(k, i.second); + } + s.endGroup(); + } + + template<typename t> + t get(const string& k) + { + return qcruft_to_t<t>(kvs[k]); + } + + void put(const string& s, const QVariant& d) + { + kvs[s] = d; + } + + bool contains(const string& s) + { + return kvs.count(s) != 0; + } + }; + + class impl_bundle : public QObject { + protected: + QMutex mtx; + const string group_name; + group saved; + group transient; + bool modified; + impl_bundle(const impl_bundle&) = delete; + impl_bundle& operator=(const impl_bundle&) = delete; + public: + impl_bundle(const string& group_name) : + mtx(QMutex::Recursive), + group_name(group_name), + saved(group_name), + transient(saved), + modified(false) + { + } + + string name() { return group_name; } + + void reload() { + QMutexLocker l(&mtx); + saved = group(group_name); + transient = saved; + modified = false; + } + + bool store_kv(const string& name, const QVariant& datum) + { + QMutexLocker l(&mtx); + + auto old = transient.get<QVariant>(name); + if (!transient.contains(name) || datum != old) + { + modified = true; + transient.put(name, datum); + return true; + } + return false; + } + bool contains(const string& name) + { + QMutexLocker l(&mtx); + return transient.contains(name); + } + template<typename t> + t get(const string& name) + { + QMutexLocker l(&mtx); + return transient.get<t>(name); + } + void save() + { + QMutexLocker l(&mtx); + modified = false; + saved = transient; + transient.save(); + } + + bool modifiedp() { + QMutexLocker l(&mtx); + return modified; + } + }; + + class opt_bundle; + + namespace + { + template<typename k, typename v, typename cnt = int> + struct opt_singleton + { + public: + using pbundle = std::shared_ptr<v>; + using tt = std::tuple<cnt, pbundle>; + private: + QMutex implsgl_mtx; + map<k, tt> implsgl_data; + public: + opt_singleton() : implsgl_mtx(QMutex::Recursive) {} + + static opt_singleton<k, v>& datum() + { + static auto ret = std::make_shared<opt_singleton<k, v>>(); + return *ret; + } + + pbundle bundle(const k& key) + { + QMutexLocker l(&implsgl_mtx); + + if (implsgl_data.count(key) != 0) + return std::get<1>(implsgl_data[key]); + + auto shr = std::make_shared<v>(key); + implsgl_data[key] = tt(cnt(1), shr); + return shr; + } + + void bundle_decf(const k& key) + { + QMutexLocker l(&implsgl_mtx); + + if (--std::get<0>(implsgl_data[key]) == 0) + implsgl_data.erase(key); + } + + ~opt_singleton() { implsgl_data.clear(); } + }; + + using pbundle = std::shared_ptr<opt_bundle>; + using t_fact = opt_singleton<string, opt_bundle>; + } + + static inline t_fact::pbundle bundle(const string name) { return t_fact::datum().bundle(name); } + + class opt_bundle : public impl_bundle + { + public: + opt_bundle() : impl_bundle("i-have-no-name") {} + opt_bundle(const string& group_name) : impl_bundle(group_name) + { + qDebug() << "bundle +" << QString::fromStdString(group_name); + } + + ~opt_bundle() + { + qDebug() << "bundle -" << QString::fromStdString(group_name); + t_fact::datum().bundle_decf(group_name); + } + }; + + class base_value : public QObject + { + Q_OBJECT +#define DEFINE_SLOT(t) void setValue(t datum) { store(datum); } +#define DEFINE_SIGNAL(t) void valueChanged(t) + public: + base_value(pbundle b, const string& name) : b(b), self_name(name) {} + signals: + DEFINE_SIGNAL(double); + DEFINE_SIGNAL(int); + DEFINE_SIGNAL(bool); + DEFINE_SIGNAL(QString); + protected: + pbundle b; + string self_name; + + template<typename t> + void store(const t& datum) + { + if (b->store_kv(self_name, datum)) + emit valueChanged(static_cast<t>(datum)); + } + public slots: + DEFINE_SLOT(double) + DEFINE_SLOT(int) + DEFINE_SLOT(QString) + DEFINE_SLOT(bool) + }; + + static inline string string_from_qstring(const QString& datum) + { + auto tmp = datum.toUtf8(); + return string(tmp.constData()); + } + + template<typename t> + class value : public base_value { + public: + t operator=(const t datum) + { + store(datum); + return datum; + } + static constexpr const Qt::ConnectionType DIRECT_CONNTYPE = Qt::DirectConnection; + static constexpr const Qt::ConnectionType SAFE_CONNTYPE = Qt::UniqueConnection; + value(pbundle b, const string& name, t def) : base_value(b, name) + { + if (!b->contains(name) || b->get<QVariant>(name).type() == QVariant::Invalid) + *this = def; + } + value(pbundle b, const QString& name, t def) : value(b, string_from_qstring(name), def) {} + value(pbundle b, const char* name, t def) : value(b, string(name), def) {} + + operator t() + { + return b->get<t>(self_name); + } + }; + + template<typename t, typename q> + inline void tie_setting(value<t>&, q*); + + template<> + inline void tie_setting(value<int>& v, QComboBox* cb) + { + cb->setCurrentIndex(v); + v = cb->currentIndex(); + base_value::connect(cb, SIGNAL(currentIndexChanged(int)), &v, SLOT(setValue(int)), v.DIRECT_CONNTYPE); + base_value::connect(&v, SIGNAL(valueChanged(int)), cb, SLOT(setCurrentIndex(int)), v.SAFE_CONNTYPE); + } + + template<> + inline void tie_setting(value<QString>& v, QComboBox* cb) + { + cb->setCurrentText(v); + v = cb->currentText(); + base_value::connect(cb, SIGNAL(currentTextChanged(QString)), &v, SLOT(setValue(QString)), v.DIRECT_CONNTYPE); + base_value::connect(&v, SIGNAL(valueChanged(QString)), cb, SLOT(setCurrentText(QString)), v.SAFE_CONNTYPE); + } + + template<> + inline void tie_setting(value<bool>& v, QCheckBox* cb) + { + cb->setChecked(v); + base_value::connect(cb, SIGNAL(toggled(bool)), &v, SLOT(setValue(bool)), v.DIRECT_CONNTYPE); + base_value::connect(&v, SIGNAL(valueChanged(bool)), cb, SLOT(setChecked(bool)), v.SAFE_CONNTYPE); + } + + template<> + inline void tie_setting(value<double>& v, QDoubleSpinBox* dsb) + { + dsb->setValue(v); + base_value::connect(dsb, SIGNAL(valueChanged(double)), &v, SLOT(setValue(double)), v.DIRECT_CONNTYPE); + base_value::connect(&v, SIGNAL(valueChanged(double)), dsb, SLOT(setValue(double)), v.SAFE_CONNTYPE); + } + + template<> + inline void tie_setting(value<int>& v, QSpinBox* sb) + { + sb->setValue(v); + base_value::connect(sb, SIGNAL(valueChanged(int)), &v, SLOT(setValue(int)), v.DIRECT_CONNTYPE); + base_value::connect(&v, SIGNAL(valueChanged(int)), sb, SLOT(setValue(int)), v.SAFE_CONNTYPE); + } + + template<> + inline void tie_setting(value<int>& v, QSlider* sl) + { + sl->setValue(v); + v = sl->value(); + base_value::connect(sl, SIGNAL(valueChanged(int)), &v, SLOT(setValue(int)), v.DIRECT_CONNTYPE); + base_value::connect(&v, SIGNAL(valueChanged(int)), sl, SLOT(setValue(int)), v.SAFE_CONNTYPE); + } + + template<> + inline void tie_setting(value<QString>& v, QLineEdit* le) + { + le->setText(v); + base_value::connect(le, SIGNAL(textChanged(QString)), &v, SLOT(setValue(QString)), v.DIRECT_CONNTYPE); + base_value::connect(&v, SIGNAL(valueChanged(QString)),le, SLOT(setText(QString)), v.SAFE_CONNTYPE); + } + + template<> + inline void tie_setting(value<QString>& v, QLabel* lb) + { + lb->setText(v); + base_value::connect(&v, SIGNAL(valueChanged(QString)), lb, SLOT(setText(QString)), v.SAFE_CONNTYPE); + } +} diff --git a/opentrack/plugin-api.hpp b/opentrack/plugin-api.hpp new file mode 100644 index 00000000..a8996d63 --- /dev/null +++ b/opentrack/plugin-api.hpp @@ -0,0 +1,11 @@ +#pragma once + +#include "export.hpp" + +enum Axis { + TX = 0, TY, TZ, Yaw, Pitch, Roll +}; + +#ifndef OPENTRACK_CROSS_ONLY +# include "plugin-qt-api.hpp" +#endif
\ No newline at end of file diff --git a/opentrack/plugin-qt-api.hpp b/opentrack/plugin-qt-api.hpp new file mode 100644 index 00000000..a2aa0d19 --- /dev/null +++ b/opentrack/plugin-qt-api.hpp @@ -0,0 +1,63 @@ +#pragma once + +#include <QString> +#include <QFrame> +#include <QIcon> + +struct Metadata +{ +public: + virtual QString name() = 0; + virtual QIcon icon() = 0; + virtual ~Metadata() {}; +}; + +// XXX TODO get rid of QString/QFrame to fix ABI woes +// will lead plugins from different C++ runtimes working -sh 20141004 + +// XXX TODO make public QWidget the mess -sh 20141004 + +struct IFilter +{ +public: + virtual ~IFilter() {}; + virtual void filter(const double *target_camera_position, double *new_camera_position) = 0; +}; + +struct IFilterDialog : public QWidget +{ + virtual ~IFilterDialog() {} + virtual void registerFilter(IFilter* tracker) = 0; + virtual void unregisterFilter() = 0; +}; + +struct IProtocol +{ +public: + virtual ~IProtocol() {}; + virtual bool correct() = 0; + virtual void pose( const double* headpose ) = 0; + virtual QString game_name() = 0; +}; + +struct IProtocolDialog : public QWidget +{ + virtual ~IProtocolDialog() {} + virtual void register_protocol(IProtocol *protocol) = 0; + virtual void unregister_protocol() = 0; +}; + +struct ITracker +{ +public: + virtual ~ITracker() {}; + virtual void start_tracker( QFrame* frame ) = 0; + virtual void data(double *data) = 0; +}; + +struct ITrackerDialog : public QWidget +{ + virtual ~ITrackerDialog() {} + virtual void register_tracker(ITracker *tracker) = 0; + virtual void unregister_tracker() = 0; +}; diff --git a/opentrack/plugin-support.cpp b/opentrack/plugin-support.cpp new file mode 100644 index 00000000..694f3e19 --- /dev/null +++ b/opentrack/plugin-support.cpp @@ -0,0 +1,215 @@ +#include <cstdio> +#include <cinttypes> +#include "plugin-support.h" +#include <QCoreApplication> +#include <QFile> +#include <QDir> + +#ifndef _WIN32 +# include <dlfcn.h> +#endif + +SelectedLibraries::~SelectedLibraries() +{ +} + +template<typename t> +static ptr<t> make_instance(ptr<dylib> lib) +{ + ptr<t> ret = nullptr; + if (lib && lib->Constructor) + ret = ptr<t>(reinterpret_cast<t*>(reinterpret_cast<CTOR_FUNPTR>(lib->Constructor)())); + qDebug() << "lib" << (lib ? lib->filename : "<null>") << "ptr" << (intptr_t)ret.get(); + return ret; +} + +SelectedLibraries::SelectedLibraries(QFrame* frame, dylibtr t, dylibtr p, dylibtr f) : + pTracker(nullptr), + pFilter(nullptr), + pProtocol(nullptr), + correct(false) +{ + pTracker = make_instance<ITracker>(t); + pProtocol = make_instance<IProtocol>(p); + pFilter = make_instance<IFilter>(f); + + if (!pTracker|| !pProtocol) + { + qDebug() << "load failure tracker" << (intptr_t)pTracker.get() << "protocol" << (intptr_t)pProtocol.get(); + return; + } + + if (pProtocol) + if(!pProtocol->correct()) + { + qDebug() << "protocol load failure"; + return; + } + + pTracker->start_tracker(frame); + + correct = true; +} + +#if defined(__APPLE__) +# define SONAME "dylib" +#elif defined(_WIN32) +# define SONAME "dll" +#else +# define SONAME "so" +#endif + +#include <iostream> + +#ifdef _MSC_VER +# error "No support for MSVC anymore" +#else +# define LIB_PREFIX "lib" +#endif + +static bool get_metadata(ptr<dylib> lib, QString& name, QIcon& icon) +{ + Metadata* meta; + if (!lib->Meta || ((meta = lib->Meta()), !meta)) + return false; + name = meta->name(); + icon = meta->icon(); + delete meta; + return true; +} + +QList<ptr<dylib>> dylib::enum_libraries() +{ +#define BASE "opentrack-" +#define SUFF "-*.*" + const char* filters_n[] = { BASE "filter" SUFF, + BASE "tracker" SUFF, + BASE "proto" SUFF }; + const Type filters_t[] = { Filter, Tracker, Protocol }; + + QDir settingsDir( QCoreApplication::applicationDirPath() ); + + QList<ptr<dylib>> ret; + + for (int i = 0; i < 3; i++) + { + QString filter = filters_n[i]; + auto t = filters_t[i]; + QStringList filenames = settingsDir.entryList(QStringList { LIB_PREFIX + filter + SONAME }, + QDir::Files, + QDir::Name); + for ( int i = 0; i < filenames.size(); i++) { + QIcon icon; + QString longName; + QString str = filenames.at(i); + auto lib = std::make_shared<dylib>(str, t); + qDebug() << "Loading" << str; + std::cout.flush(); + if (!get_metadata(lib, longName, icon)) + continue; + ret.push_back(lib); + } + } + + return ret; +} + +dylib::dylib(const QString& filename, Type t) : + type(t), + Dialog(nullptr), + Constructor(nullptr), + Meta(nullptr) +{ + this->filename = filename; +#if defined(_WIN32) + QString fullPath = QCoreApplication::applicationDirPath() + "/" + this->filename; + handle = new QLibrary(fullPath); + + struct _foo { + static bool die(QLibrary*& l, bool failp) + { + if (failp) + { + qDebug() << "failed" << l->errorString(); + delete l; + l = nullptr; + } + return failp; + } + }; + + if (_foo::die(handle, !handle->load())) + return; + + Dialog = (CTOR_FUNPTR) handle->resolve("GetDialog"); + if (_foo::die(handle, !Dialog)) + return; + + Constructor = (CTOR_FUNPTR) handle->resolve("GetConstructor"); + if (_foo::die(handle, !Constructor)) + return; + + Meta = (METADATA_FUNPTR) handle->resolve("GetMetadata"); + if (_foo::die(handle, !Meta)) + return; +#else + QByteArray latin1 = QFile::encodeName(filename); + handle = dlopen(latin1.constData(), RTLD_NOW | +# ifdef __linux + RTLD_DEEPBIND +# elif defined(__APPLE__) + RTLD_LOCAL|RTLD_FIRST|RTLD_NOW +# else + 0 +# endif + ); + + struct _foo { + static bool err(void*& handle) + { + const char* err = dlerror(); + if (err) + { + fprintf(stderr, "Error, ignoring: %s\n", err); + fflush(stderr); + dlclose(handle); + handle = nullptr; + return true; + } + return false; + } + }; + + if (handle) + { + if (_foo::err(handle)) + return; + Dialog = (CTOR_FUNPTR) dlsym(handle, "GetDialog"); + if (_foo::err(handle)) + return; + Constructor = (CTOR_FUNPTR) dlsym(handle, "GetConstructor"); + if (_foo::err(handle)) + return; + Meta = (METADATA_FUNPTR) dlsym(handle, "GetMetadata"); + if (_foo::err(handle)) + return; + } else { + (void) _foo::err(handle); + } +#endif + + auto m = ptr<Metadata>(Meta()); + + icon = m->icon(); + name = m->name(); +} + +dylib::~dylib() +{ +#if defined(_WIN32) + handle->unload(); +#else + if (handle) + (void) dlclose(handle); +#endif +} diff --git a/opentrack/plugin-support.h b/opentrack/plugin-support.h new file mode 100644 index 00000000..238aeb53 --- /dev/null +++ b/opentrack/plugin-support.h @@ -0,0 +1,51 @@ +#pragma once + +#include "plugin-api.hpp" + +#include <QWidget> +#include <QDebug> +#include <QString> +#include <QLibrary> +#include <QFrame> +#include <QList> + +#include <memory> +template<typename t> using ptr = std::shared_ptr<t>; + +extern "C" typedef void* (*CTOR_FUNPTR)(void); +extern "C" typedef Metadata* (*METADATA_FUNPTR)(void); + +struct dylib { + enum Type { Filter, Tracker, Protocol }; + + dylib(const QString& filename, Type t); + ~dylib(); + static QList<ptr<dylib>> enum_libraries(); + + Type type; + QString filename; + + QIcon icon; + QString name; + + CTOR_FUNPTR Dialog; + CTOR_FUNPTR Constructor; + METADATA_FUNPTR Meta; +private: +#if defined(_WIN32) + QLibrary* handle; +#else + void* handle; +#endif +}; + +struct SelectedLibraries { + using dylibtr = ptr<dylib>; + ptr<ITracker> pTracker; + ptr<IFilter> pFilter; + ptr<IProtocol> pProtocol; + SelectedLibraries(QFrame* frame, dylibtr t, dylibtr p, dylibtr f); + SelectedLibraries() : pTracker(nullptr), pFilter(nullptr), pProtocol(nullptr), correct(false) {} + ~SelectedLibraries(); + bool correct; +};
\ No newline at end of file diff --git a/opentrack/pose.hpp b/opentrack/pose.hpp new file mode 100644 index 00000000..41e984f5 --- /dev/null +++ b/opentrack/pose.hpp @@ -0,0 +1,44 @@ +#pragma once + +#include <utility> +#include <algorithm> +#include "./quat.hpp" +#include "./plugin-api.hpp" + +class Pose { +private: + static constexpr double pi = 3.141592653; + static constexpr double d2r = pi/180.0; + static constexpr double r2d = 180./pi; + + double axes[6]; +public: + Pose() : axes {0,0,0, 0,0,0 } {} + + inline operator double*() { return axes; } + inline operator const double*() const { return axes; } + + inline double& operator()(int i) { return axes[i]; } + inline double operator()(int i) const { return axes[i]; } + + Quat quat() const + { + return Quat(axes[Yaw]*d2r, axes[Pitch]*d2r, axes[Roll]*d2r); + } + + static Pose fromQuat(const Quat& q) + { + Pose ret; + q.to_euler_degrees(ret(Yaw), ret(Pitch), ret(Roll)); + return ret; + } + + Pose operator&(const Pose& B) const + { + const Quat q = quat() * B.quat().inv(); + Pose ret = fromQuat(q); + for (int i = TX; i < TX + 3; i++) + ret(i) = axes[i] - B.axes[i]; + return ret; + } +}; diff --git a/opentrack/qcopyable-mutex.hpp b/opentrack/qcopyable-mutex.hpp new file mode 100644 index 00000000..f7f36f93 --- /dev/null +++ b/opentrack/qcopyable-mutex.hpp @@ -0,0 +1,37 @@ +#pragma once + +#include <QMutex> + +class MyMutex { +private: + QMutex inner; + +public: + QMutex* operator->() { return &inner; } + QMutex* operator->() const { return &const_cast<MyMutex*>(this)->inner; } + + MyMutex operator=(const MyMutex& datum) + { + auto mode = + datum->isRecursive() + ? QMutex::Recursive + : QMutex::NonRecursive; + + return MyMutex(mode); + } + + MyMutex(const MyMutex& datum) + { + *this = datum; + } + + MyMutex(QMutex::RecursionMode mode = QMutex::NonRecursive) : + inner(mode) + { + } + + QMutex* operator&() + { + return &inner; + } +}; diff --git a/opentrack/quat.hpp b/opentrack/quat.hpp new file mode 100644 index 00000000..6d777b28 --- /dev/null +++ b/opentrack/quat.hpp @@ -0,0 +1,66 @@ +/* Copyright (c) 2012 Patrick Ruoff + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#pragma once +#include <cmath> + +class Quat { +private: + static constexpr double pi = 3.141592653; + static constexpr double r2d = 180./pi; + double a,b,c,d; // quaternion coefficients +public: + Quat() : a(1.),b(0.),c(0.),d(0.) {} + Quat(double yaw, double pitch, double roll) { from_euler_rads(yaw, pitch, roll); } + Quat(double a, double b, double c, double d) : a(a),b(b),c(c),d(d) {} + + Quat inv(){ + return Quat(a,-b,-c, -d); + } + + // conversions + // see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles + void from_euler_rads(double yaw, double pitch, double roll) + { + + const double sin_phi = sin(roll/2.); + const double cos_phi = cos(roll/2.); + const double sin_the = sin(pitch/2.); + const double cos_the = cos(pitch/2.); + const double sin_psi = sin(yaw/2.); + const double cos_psi = cos(yaw/2.); + + a = cos_phi*cos_the*cos_psi + sin_phi*sin_the*sin_psi; + b = sin_phi*cos_the*cos_psi - cos_phi*sin_the*sin_psi; + c = cos_phi*sin_the*cos_psi + sin_phi*cos_the*sin_psi; + d = cos_phi*cos_the*sin_psi - sin_phi*sin_the*cos_psi; + } + + void to_euler_rads(double& yaw, double& pitch, double& roll) const + { + roll = atan2(2.*(a*b + c*d), 1. - 2.*(b*b + c*c)); + pitch = asin(2.*(a*c - b*d)); + yaw = atan2(2.*(a*d + b*c), 1. - 2.*(c*c + d*d)); + } + + void to_euler_degrees(double& yaw, double& pitch, double& roll) const + { + to_euler_rads(yaw, pitch, roll); + yaw *= r2d; + pitch *= r2d; + roll *= r2d; + } + + const Quat operator*(const Quat& B) const + { + const Quat& A = *this; + return Quat(A.a*B.a - A.b*B.b - A.c*B.c - A.d*B.d, // quaternion multiplication + A.a*B.b + A.b*B.a + A.c*B.d - A.d*B.c, + A.a*B.c - A.b*B.d + A.c*B.a + A.d*B.b, + A.a*B.d + A.b*B.c - A.c*B.b + A.d*B.a); + } +}; diff --git a/opentrack/timer.hpp b/opentrack/timer.hpp new file mode 100644 index 00000000..628365c9 --- /dev/null +++ b/opentrack/timer.hpp @@ -0,0 +1,68 @@ +#pragma once +#include <ctime> +#if defined (_WIN32) +# include <windows.h> +static inline void opentrack_clock_gettime(int, struct timespec* ts) +{ + static LARGE_INTEGER freq; + + if (!freq.QuadPart) + (void) QueryPerformanceFrequency(&freq); + + LARGE_INTEGER d; + + (void) QueryPerformanceCounter(&d); + + d.QuadPart *= 1000000000L; + d.QuadPart /= freq.QuadPart; + + ts->tv_sec = d.QuadPart / 1000000000L; + ts->tv_nsec = d.QuadPart % 1000000000L; +} +# define clock_gettime opentrack_clock_gettime +#else +# if defined(__MACH__) +# define CLOCK_MONOTONIC 0 +# include <inttypes.h> +# include <mach/mach_time.h> +static inline void clock_gettime(int, struct timespec* ts) +{ + static mach_timebase_info_data_t sTimebaseInfo; + uint64_t state, nsec; + if ( sTimebaseInfo.denom == 0 ) { + (void) mach_timebase_info(&sTimebaseInfo); + } + state = mach_absolute_time(); + nsec = state * sTimebaseInfo.numer / sTimebaseInfo.denom; + ts->tv_sec = nsec / 1000000000L; + ts->tv_nsec = nsec % 1000000000L; +} +# endif +#endif +class Timer { +private: + struct timespec state; + long conv(const struct timespec& cur) + { + return (cur.tv_sec - state.tv_sec) * 1000000000L + (cur.tv_nsec - state.tv_nsec); + } +public: + Timer() { + start(); + } + long start() { + struct timespec cur; + (void) clock_gettime(CLOCK_MONOTONIC, &cur); + long ret = conv(cur); + state = cur; + return ret; + } + long elapsed() { + struct timespec cur; + (void) clock_gettime(CLOCK_MONOTONIC, &cur); + return conv(cur); + } + long elapsed_ms() { + return elapsed() / 1000000L; + } +}; diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp new file mode 100644 index 00000000..41db7c19 --- /dev/null +++ b/opentrack/tracker.cpp @@ -0,0 +1,187 @@ +/* Copyright (c) 2012-2013 Stanislaw Halik <sthalik@misaki.pl> + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +/* + * this file appeared originally in facetracknoir, was rewritten completely + * following opentrack fork. + * + * originally written by Wim Vriend. + */ + +#include <opencv2/core/core.hpp> + +#include "./tracker.h" +#include <cmath> +#include <algorithm> + +#if defined(_WIN32) +# include <windows.h> +#endif + +Tracker::Tracker(main_settings& s, Mappings &m, SelectedLibraries &libs) : + s(s), + m(m), + centerp(false), + enabledp(true), + should_quit(false), + libs(libs) +{ +} + +Tracker::~Tracker() +{ + should_quit = true; + wait(); +} + +double Tracker::map(double pos, Mapping& axis) { + bool altp = (pos < 0) && axis.opts.altp; + axis.curve.setTrackingActive( !altp ); + axis.curveAlt.setTrackingActive( altp ); + auto& fc = altp ? axis.curveAlt : axis.curve; + double invert = axis.opts.invert ? -1 : 1; + return invert * (fc.getValue(pos) + axis.opts.zero); +} + +static cv::Matx33d euler_to_rmat(const double* input) +{ + static constexpr double pi = 3.141592653; + const auto H = input[0] * pi / -180; + const auto P = input[1] * pi / -180; + const auto B = input[2] * pi / 180; + + const auto cosH = cos(H); + const auto sinH = sin(H); + const auto cosP = cos(P); + const auto sinP = sin(P); + const auto cosB = cos(B); + const auto sinB = sin(B); + + double foo[] = { + cosH * cosB - sinH * sinP * sinB, + - sinB * cosP, + sinH * cosB + cosH * sinP * sinB, + cosH * sinB + sinH * sinP * cosB, + cosB * cosP, + sinB * sinH - cosH * sinP * cosB, + - sinH * cosP, + - sinP, + cosH * cosP, + }; + + return cv::Matx33d(foo); +} + +void Tracker::t_compensate(const double* input, double* output, bool rz) +{ + const cv::Matx33d rmat = euler_to_rmat(&input[Yaw]); + const cv::Vec3d tvec(input); + const cv::Vec3d ret = rmat * tvec; + + const int max = !rz ? 3 : 2; + + for (int i = 0; i < max; i++) + output[i] = ret(i); +} + +void Tracker::logic() +{ + libs.pTracker->data(newpose); + + Pose final_raw; + + if (enabledp) + { + for (int i = 0; i < 6; i++) + { + auto& axis = m(i); + int k = axis.opts.src; + if (k < 0 || k >= 6) + { + final_raw(i) = 0; + continue; + } + // not really raw, after axis remap -sh + final_raw(i) = newpose[k]; + } + unstopped_raw = final_raw; + } + + Pose filtered_pose; + + if (libs.pFilter) + libs.pFilter->filter(final_raw, filtered_pose); + else + filtered_pose = final_raw; + + if (centerp) + { + centerp = false; + raw_center = final_raw; + } + + Pose raw_centered = filtered_pose & raw_center; + + Pose mapped_pose_precomp; + + for (int i = 0; i < 6; i++) + mapped_pose_precomp(i) = map(raw_centered(i), m(i)); + + Pose mapped_pose; + + mapped_pose = mapped_pose_precomp; + if (s.tcomp_p) + t_compensate(mapped_pose_precomp, mapped_pose, s.tcomp_tz); + + libs.pProtocol->pose(mapped_pose); + + { + QMutexLocker foo(&mtx); + output_pose = mapped_pose; + raw_6dof = unstopped_raw; + } +} + +void Tracker::run() { + const int sleep_ms = 3; + +#if defined(_WIN32) + (void) timeBeginPeriod(1); +#endif + + while (!should_quit) + { + t.start(); + + logic(); + + double q = sleep_ms * 1000L; + q -= t.elapsed(); + q = std::max(0., q); + usleep((long)q); + } + +#if defined(_WIN32) + (void) timeEndPeriod(1); +#endif + + for (int i = 0; i < 6; i++) + { + m(i).curve.setTrackingActive(false); + m(i).curveAlt.setTrackingActive(false); + } +} + +void Tracker::get_raw_and_mapped_poses(double* mapped, double* raw) const { + QMutexLocker foo(&const_cast<Tracker&>(*this).mtx); + for (int i = 0; i < 6; i++) + { + raw[i] = raw_6dof(i); + mapped[i] = output_pose(i); + } +} + diff --git a/opentrack/tracker.h b/opentrack/tracker.h new file mode 100644 index 00000000..02d6bee2 --- /dev/null +++ b/opentrack/tracker.h @@ -0,0 +1,47 @@ +#pragma once + +#include <atomic> +#include <vector> + +#include "./timer.hpp" +#include "./plugin-support.h" +#include "./mappings.hpp" +#include "./pose.hpp" + +#include "../qfunctionconfigurator/functionconfig.h" +#include "./main-settings.hpp" +#include "./options.hpp" + +#include <QMutex> +#include <QThread> + +class Tracker : private QThread { + Q_OBJECT +private: + QMutex mtx; + main_settings& s; + // XXX can be const-cast when functionconfig const-correct -sh 20141004 + Mappings& m; + + Timer t; + Pose output_pose, raw_6dof, raw_center, unstopped_raw; + double newpose[6]; + std::atomic<bool> centerp; + std::atomic<bool> enabledp; + std::atomic<bool> should_quit; + SelectedLibraries const& libs; + + double map(double pos, Mapping& axis); + void logic(); + + static void t_compensate(const double* input, double* output, bool rz); + void run() override; +public: + Tracker(main_settings& s, Mappings& m, SelectedLibraries& libs); + ~Tracker(); + + void get_raw_and_mapped_poses(double* mapped, double* raw) const; + void start() { QThread::start(); } + void toggle_enabled() { enabledp.store(!enabledp.load()); } + void center() { centerp.store(!centerp.load()); } +}; |