diff options
Diffstat (limited to 'FTNoIR_Filter_DZ1/ftnoir_filter_dz1.cpp')
| -rw-r--r-- | FTNoIR_Filter_DZ1/ftnoir_filter_dz1.cpp | 251 |
1 files changed, 251 insertions, 0 deletions
diff --git a/FTNoIR_Filter_DZ1/ftnoir_filter_dz1.cpp b/FTNoIR_Filter_DZ1/ftnoir_filter_dz1.cpp new file mode 100644 index 00000000..ce1b4063 --- /dev/null +++ b/FTNoIR_Filter_DZ1/ftnoir_filter_dz1.cpp @@ -0,0 +1,251 @@ +/********************************************************************************
+* FaceTrackNoIR This program is a private project of some enthusiastic *
+* gamers from Holland, who don't like to pay much for *
+* head-tracking. *
+* *
+* Copyright (C) 2012 Wim Vriend (Developing) *
+* Ron Hendriks (Researching and Testing) *
+* *
+* Homepage *
+* *
+* This program is free software; you can redistribute it and/or modify it *
+* under the terms of the GNU General Public License as published by the *
+* Free Software Foundation; either version 3 of the License, or (at your *
+* option) any later version. *
+* *
+* This program is distributed in the hope that it will be useful, but *
+* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY *
+* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for *
+* more details. *
+* *
+* You should have received a copy of the GNU General Public License along *
+* with this program; if not, see <http://www.gnu.org/licenses/>. *
+* *
+********************************************************************************/
+#include "ftnoir_filter_DZ1.h"
+#include "math.h"
+#include <QDebug>
+
+//#define LOG_OUTPUT
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//
+// EWMA Filter: Exponentially Weighted Moving Average filter with dynamic smoothing parameter
+//
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+FTNoIR_Filter::FTNoIR_Filter()
+{
+ //populate the description strings
+ filterFullName = "Deadzone Filter Mk1";
+ filterShortName = "DZ1";
+ filterDescription = "Deadzone Filter";
+
+ first_run = true;
+ loadSettings(); // Load the Settings
+
+}
+
+FTNoIR_Filter::~FTNoIR_Filter()
+{
+
+}
+
+void FTNoIR_Filter::Release()
+{
+ delete this;
+}
+
+void FTNoIR_Filter::Initialize()
+{
+ qDebug() << "FTNoIR_Filter::Initialize says: Starting ";
+ loadSettings();
+ return;
+}
+
+//
+// Load the current Settings from the currently 'active' INI-file.
+//
+void FTNoIR_Filter::loadSettings() {
+ qDebug() << "FTNoIR_Filter::loadSettings says: Starting ";
+ QSettings settings("Abbequerque Inc.", "FaceTrackNoIR"); // Registry settings (in HK_USER)
+
+ QString currentFile = settings.value ( "SettingsFile", QCoreApplication::applicationDirPath() + "/Settings/default.ini" ).toString();
+ QSettings iniFile( currentFile, QSettings::IniFormat ); // Application settings (in INI-file)
+
+ qDebug() << "FTNoIR_Filter::loadSettings says: iniFile = " << currentFile;
+
+ //
+ // The EWMA2-filter-settings are in the Tracking group: this is because they used to be on the Main Form of FaceTrackNoIR
+ //
+ iniFile.beginGroup ( "Filter_DZ1" );
+ kCameraHz = iniFile.value ( "cameraHz", 30 ).toInt();
+ kDeadZone = iniFile.value ( "DeadZone", 0.1f ).toFloat();
+ kMoveLast = (iniFile.value ( "MoveLast", 24 ).toFloat()) / 100; // Convert from int to float percentage
+ kMaxDiff = iniFile.value ( "MaxDiff", 1.75f ).toFloat();
+ kMoveSaved = ((iniFile.value ( "MoveSaved", 35 ).toFloat()) / 100) / kCameraHz; // Convert from int to float percentage and divide by Hz
+ iniFile.endGroup ();
+
+}
+
+void FTNoIR_Filter::FilterHeadPoseData(THeadPoseData *current_camera_position, THeadPoseData *target_camera_position, THeadPoseData *new_camera_position, bool newTarget)
+{
+ //non-optimised version for clarity + double target[6]; + double prev_output[6]; + double scale[]={3.0, 3.0, 3.0, 3.0, 3.0, 3.0}; +// static double deadzones[] = { DEADZONE, DEADZONE, DEADZONE, DEADZONE, DEADZONE, DEADZONE }; + float output[6]; + int i=0, j=0; + + //find out how far the head has moved + prev_output[0]=current_camera_position->x; + prev_output[1]=current_camera_position->y; + prev_output[2]=current_camera_position->z; + prev_output[3]=current_camera_position->yaw; + prev_output[4]=current_camera_position->pitch; + prev_output[5]=current_camera_position->roll; + + target[0]=target_camera_position->x; + target[1]=target_camera_position->y; + target[2]=target_camera_position->z; + target[3]=target_camera_position->yaw; + target[4]=target_camera_position->pitch; + target[5]=target_camera_position->roll; + + if (first_run) + { + //on the first run, output=target + for (i=0;i<6;i++) + { + last_positions[i] = target[i]; + saved_positions[i] = target[i]; + prev_positions[i] = target[i]; + smooth_remember[i] = 0; + smoothing[i] = false; + smooth_speed[i] = -1.0; + } + + new_camera_position->x=target[0]; + new_camera_position->y=target[1]; + new_camera_position->z=target[2]; + new_camera_position->yaw=target[3]; + new_camera_position->pitch=target[4]; + new_camera_position->roll=target[5]; + + first_run=false; + + //we can bail + return; + } else if (!newTarget) { + int i; + for (i = 0; i < 6; i++) { + output[i] = prev_positions[i]; + } + goto end; + } + + for (i=0;i<6;i++) + { + double e2 = target[i]; + bool slowp = fabs(last_positions[i] - e2) < kDeadZone && fabs(e2 - saved_positions[i]) < kMaxDiff; // DeadZone and MAXDIFF from INI-file + if (smoothing[i] || !slowp) { + double start = smooth_start[i]; + double vec = e2 - start; + int sign = vec < 0 ? -1 : 1; + int sign2 = e2 < 0 ? -1 : 1; + if (sign != sign2) + sign2 = -sign2; + double diff = (kCameraHz / REMEMBER_SMOOTHNESS) - smooth_remember[i]; + smooth_speed[i] *= -(diff * diff) / (double) ((kCameraHz / REMEMBER_SMOOTHNESS) * (kCameraHz / REMEMBER_SMOOTHNESS)) + 1.001; + if (smooth_speed[i] < INITIAL_SMOOTH_SPEED) + smooth_speed[i] = INITIAL_SMOOTH_SPEED; + vec = fabs(vec); + double iter = vec * SMOOTH_FACTOR / (double) kCameraHz; + smooth_speed[i] += iter; + double bleh = smooth_speed[i] * sign; + double foo = smooth_start[i] + bleh; + bool done = sign2 > 0 ? foo > e2 : foo < e2; + smooth_remember[i] = (kCameraHz / REMEMBER_SMOOTHNESS); + if (done) { + smoothing[i] = false; + output[i] = e2; + saved_positions[i] = e2; + last_positions[i] = e2; + prev_positions[i] = e2; + } else { + smoothing[i] = true; + prev_positions[i] = output[i] = foo; + } + } else { + if (smooth_remember[i] <= 0) { + smooth_speed[i] = -1.0; + } else { + smooth_remember[i]--; + } + last_positions[i] = last_positions[i] + (e2 - last_positions[i]) * kMoveLast; // MOVE_LAST from INI-file + saved_positions[i] = saved_positions[i] + (e2 - saved_positions[i]) * kMoveSaved; // MOVE_SAVED from INI-file + output[i] = prev_positions[i] = prev_positions[i] + (e2 - prev_positions[i]) * (SLOW_SPEED/kCameraHz); // Get CameraHz from INI + smooth_start[i] = output[i]; + smoothing[i] = false; +#if 0 + if (i == 3) { + FILE* dlog = fopen("debug-log.txt", "a"); + fprintf(dlog, "slowed=%f frames=%d diff=%f\n", output[i], slow_move_count[i], e1 - last_positions[i]); + fclose(dlog); + } +#endif + } + } + +end: + new_camera_position->x=output[0]; + new_camera_position->y=output[1]; + new_camera_position->z=output[2]; + new_camera_position->yaw=output[3] * MULT_X; + new_camera_position->pitch=output[4] * (output[4] < 0 ? MULT_Y_NEG : MULT_Y_POS) - COCKPIT_PITCH; + new_camera_position->roll=output[5]; + +#if 1 + // + // Also update the 'current' position, for the next iteration. + // + current_camera_position->x=output[0]; + current_camera_position->y=output[1]; + current_camera_position->z=output[2]; + current_camera_position->yaw=output[3] * MULT_X; + current_camera_position->pitch=output[4] * (output[4] < 0 ? MULT_Y_NEG : MULT_Y_POS) + COCKPIT_PITCH; + current_camera_position->roll=output[5]; +#endif +}
+
+void FTNoIR_Filter::getFilterFullName(QString *strToBeFilled)
+{
+ *strToBeFilled = filterFullName;
+};
+
+
+void FTNoIR_Filter::getFilterShortName(QString *strToBeFilled)
+{
+ *strToBeFilled = filterShortName;
+};
+
+
+void FTNoIR_Filter::getFilterDescription(QString *strToBeFilled)
+{
+ *strToBeFilled = filterDescription;
+};
+
+////////////////////////////////////////////////////////////////////////////////
+// Factory function that creates instances if the Filter object.
+
+// Export both decorated and undecorated names.
+// GetFilter - Undecorated name, which can be easily used with GetProcAddress
+// Win32 API function.
+// _GetFilter@0 - Common name decoration for __stdcall functions in C language.
+#pragma comment(linker, "/export:GetFilter=_GetFilter@0")
+
+FTNOIR_FILTER_BASE_EXPORT FILTERHANDLE __stdcall GetFilter()
+{
+ return new FTNoIR_Filter;
+}
|