From 556e81afeb52930a736306310c863743dc5b8030 Mon Sep 17 00:00:00 2001 From: Jonathan Bosson Date: Tue, 25 Apr 2017 13:11:40 -0600 Subject: [PATCH] fix diagonal-orbit-only bug and cleanup of code --- ext/ghoul | 2 +- modules/touch/ext/levmarq.cpp | 16 +--- modules/touch/ext/levmarq.h | 2 +- modules/touch/src/TouchInteraction.cpp | 103 ++++++++++--------------- modules/touch/touchmodule.cpp | 31 +------- 5 files changed, 49 insertions(+), 105 deletions(-) diff --git a/ext/ghoul b/ext/ghoul index 20131b6897..894358db7a 160000 --- a/ext/ghoul +++ b/ext/ghoul @@ -1 +1 @@ -Subproject commit 20131b68975456d4a90b5cf997dd8f10179a3a31 +Subproject commit 894358db7a1e15d7ae81a251aa2062ae8a48fa87 diff --git a/modules/touch/ext/levmarq.cpp b/modules/touch/ext/levmarq.cpp index 6f690930dc..a179fb9ec5 100644 --- a/modules/touch/ext/levmarq.cpp +++ b/modules/touch/ext/levmarq.cpp @@ -30,7 +30,7 @@ OTHER DEALINGS IN THE SOFTWARE. // set parameters required by levmarq() to default values void levmarq_init(LMstat *lmstat) { - lmstat->verbose = 0; + lmstat->verbose = 1; lmstat->max_it = 5000; lmstat->init_lambda = 1e-6; lmstat->up_factor = 10; @@ -57,7 +57,7 @@ The arguments are as follows: Before calling levmarq, several of the parameters in lmstat must be set. For default values, call levmarq_init(lmstat). */ -int levmarq(int npar, double *par, int ny, double* y, double *dysq, +bool levmarq(int npar, double *par, int ny, double *dysq, double (*func)(double *, int, void *), void (*grad)(double *, double *, int, void *), void *fdata, LMstat *lmstat) { @@ -123,15 +123,7 @@ int levmarq(int npar, double *par, int ny, double* y, double *dysq, ill = (derr > 0); } if (verbose) { - printf("it = %4d, newerror = %10g, err = %10g, derr = %10g\n", it, newerr, err, derr); - for (i = 0; i < npar; i++) { - printf("%f:", par[i]); - } - printf("\n"); - for (i = 0; i < npar; ++i) { - printf("%f:", delta[i]); - } - printf("\n"); + printf("it = %4d, lambda = %10g, err = %10g, derr = %10g (%d)\n", it, lambda, err, derr, !(newerr > err)); } if (ill) { mult = (1 + lambda * up) / (1 + lambda); @@ -164,7 +156,7 @@ int levmarq(int npar, double *par, int ny, double* y, double *dysq, delete[] delta; delete[] newpar; - return it; + return (it != lmstat->max_it); } diff --git a/modules/touch/ext/levmarq.h b/modules/touch/ext/levmarq.h index 4dad30e0a1..0009cc1324 100644 --- a/modules/touch/ext/levmarq.h +++ b/modules/touch/ext/levmarq.h @@ -38,7 +38,7 @@ typedef struct { void levmarq_init(LMstat *lmstat); -int levmarq(int npar, double *par, int ny, double* y, double *dysq, +bool levmarq(int npar, double *par, int ny, double *dysq, double (*func)(double *, int, void *), void (*grad)(double *, double *, int, void *), void *fdata, LMstat *lmstat); diff --git a/modules/touch/src/TouchInteraction.cpp b/modules/touch/src/TouchInteraction.cpp index 17893d7d0f..1198f3e62a 100644 --- a/modules/touch/src/TouchInteraction.cpp +++ b/modules/touch/src/TouchInteraction.cpp @@ -82,15 +82,16 @@ TouchInteraction::TouchInteraction() TouchInteraction::~TouchInteraction() { } void TouchInteraction::update(const std::vector& list, std::vector& lastProcessed) { - if (_directTouchMode && _selected.size() > 0 && list.size() == _selected.size()) { // should just be a function call + if (_directTouchMode && _selected.size() > 0 && list.size() == _selected.size()) { // code below should just be a function call + // Returns the screen point s(xi,par) dependant the transform M(par) and object point xi auto distToMinimize = [](double* par, int x, void* fdata) { FunctionData* ptr = reinterpret_cast(fdata); // Apply transform to camera and find the new screen point of the updated camera state - double vel[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; // { vec2 globalRot, zoom, roll, vec2 localRot } + double q[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; // { vec2 globalRot, zoom, roll, vec2 localRot } for (int i = 0; i < ptr->nDOF; ++i) - vel[i] = par[i]; + q[i] = par[i]; using namespace glm; // Create variables from current state @@ -111,16 +112,16 @@ void TouchInteraction::update(const std::vector& list, std::vector

camera->rotationQuaternion(); { // Roll - dquat camRollRot = angleAxis(vel[3], dvec3(0.0, 0.0, 1.0)); + dquat camRollRot = angleAxis(q[3], dvec3(0.0, 0.0, 1.0)); localCamRot = localCamRot * camRollRot; } { // Panning (local rotation) - dvec3 eulerAngles(vel[5], vel[4], 0); + dvec3 eulerAngles(q[5], q[4], 0); dquat rotationDiff = dquat(eulerAngles); localCamRot = localCamRot * rotationDiff; } { // Orbit (global rotation) - dvec3 eulerAngles(vel[1], vel[0], 0); + dvec3 eulerAngles(q[1], q[0], 0); dquat rotationDiffCamSpace = dquat(eulerAngles); dquat rotationDiffWorldSpace = globalCamRot * rotationDiffCamSpace * inverse(globalCamRot); @@ -137,7 +138,7 @@ void TouchInteraction::update(const std::vector& list, std::vector

& list, std::vector

castToNDC(ptr->selectedPoints.at(x), cam, ptr->node, ptr->aspectRatio); - - /* 2 DOF = trans in XY, 4DOF = trans XYZ + roll case - // Create transformation matrix M(q) and apply transform for newPointInModelView - glm::dvec3 T = glm::dvec3(par[0], par[1], 0.0); - glm::dvec3 eulerAngles(0.0, 0.0, 0.0); - double twoPi = 2.0 * M_PI; - if (ptr->nDOF > 2) { // need to check how many DOF we can handle (+2 DOF per finger down up to 6) - T.z = par[2]; - eulerAngles.z = fmod(par[3], twoPi); - if (ptr->nDOF > 4) { - eulerAngles.y = fmod(par[4], twoPi); - eulerAngles.x = fmod(par[5], twoPi); - } - } - glm::dquat Q = glm::normalize(glm::dquat(eulerAngles)); - glm::dvec3 pointInCamSpace = glm::inverse(ptr->camera->rotationQuaternion()) * (ptr->node->rotationMatrix() * selectedPoint); - glm::dvec3 newWorldSP = (Q * pointInCamSpace) + T; // cam space around origin - - glm::dvec2 newScreenPoint = ptr->castToScreen(newWorldSP, ptr->camera, ptr->node, ptr->aspectRatio);*/ return glm::length(ptr->screenPoints.at(x) - newScreenPoint); }; @@ -173,23 +155,23 @@ void TouchInteraction::update(const std::vector& list, std::vector

(fdata); double f0 = ptr->distToMinimize(par, x, fdata); - double f1, h = 1e-4; + double f1, der, h; double* dPar = new double[ptr->nDOF]; - + for (int i = 0; i < ptr->nDOF; ++i) { for (int j = 0; j < ptr->nDOF; ++j) { dPar[j] = par[j]; } + h = (i == 2) ? 1e-4: 1e-10; dPar[i] += h; f1 = ptr->distToMinimize(dPar, x, fdata); - g[i] = (f1 - f0) / h; + der = (f1 - f0) / h; + g[i] = (i > 1 && i < 4) ? der : der / abs(der); } delete[] dPar; }; SceneGraphNode* node = _selected.at(0).node; - glm::dvec2 res = OsEng.windowWrapper().currentWindowResolution(); - double aspectRatio = res.x / res.y; auto castToNDC = [](glm::dvec3 vec, Camera& camera, SceneGraphNode* node, double aspectRatio) { glm::dvec3 backToScreenSpace = glm::inverse(camera.rotationQuaternion()) * glm::normalize(((node->rotationMatrix() * vec) + node->worldPosition() - camera.positionVec3())); @@ -203,8 +185,9 @@ void TouchInteraction::update(const std::vector& list, std::vector

selectedPoints; std::vector screenPoints; for (const SelectedBody& sb : _selected) { @@ -215,39 +198,34 @@ void TouchInteraction::update(const std::vector& list, std::vector

getY() - 0.5); // normalized -1 to 1 coordinates on screen screenPoints.push_back(glm::dvec2(xCo, yCo)); } - double* squaredError = new double[nFingers]; // probably not needed - for (int i = 0; i < nFingers; ++i) { - double err = glm::length(screenPoints.at(i) - castToNDC(selectedPoints.at(i), *_camera, node, aspectRatio)); - squaredError[i] = err; - } - - FunctionData fData = { selectedPoints, screenPoints, nDOF, castToNDC, distToMinimize, _camera, node, aspectRatio }; + glm::dvec2 res = OsEng.windowWrapper().currentWindowResolution(); + FunctionData fData = { selectedPoints, screenPoints, nDOF, castToNDC, distToMinimize, _camera, node, res.x / res.y }; void* dataPtr = reinterpret_cast(&fData); levmarq_init(&_lmstat); - int nIterations = levmarq(nDOF, par, nFingers, squaredError, NULL, distToMinimize, gradient, dataPtr, &_lmstat); // finds best transform values and stores them in par + bool success = levmarq(nDOF, par, nFingers, NULL, distToMinimize, gradient, dataPtr, &_lmstat); // finds best transform values and stores them in par - double temp[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - for (int i = 0; i < nDOF; ++i) - temp[i] = par[i]; + if (success) { // if good values were found set new camera state + _vel.globalRot = glm::dvec2(par[0], par[1]); + if (nDOF > 2) { + _vel.zoom = par[2]; + _vel.localRoll = par[3]; + if (nDOF > 4) { + _vel.localRot = glm::dvec2(par[4], par[5]); + } + } + step(1); + } - // Set the new camera state - _vel.globalRot = glm::dvec2(temp[0], temp[1]); - _vel.zoom = temp[2]; - _vel.localRoll = temp[3]; - _vel.localRot = glm::dvec2(temp[4], temp[5]); - step(1); - // debugging - std::ostringstream os; + /*std::ostringstream os; for (int i = 0; i < nDOF; ++i) { - os << temp[i] << ", "; + os << par[i] << ", "; } - std::cout << "Levmarq success after " << nIterations << " iterations. Values: " << os.str() << "\n"; + std::cout << "Levmarq success after " << _lmstat.final_it << " iterations. Values: " << os.str() << "\n";*/ // cleanup - delete[] squaredError; delete[] par; } trace(list); @@ -426,9 +404,9 @@ void TouchInteraction::accelerate(const std::vector& list, const std double res = diff; double lastAngle = point.getAngle(_centroid.x, _centroid.y); double currentAngle = c.getAngle(_centroid.x, _centroid.y); - if (lastAngle > currentAngle + 1.5*M_PI) + if (lastAngle > currentAngle + 1.5 * M_PI) res += currentAngle + (2 * M_PI - lastAngle); - else if (currentAngle > lastAngle + 1.5*M_PI) + else if (currentAngle > lastAngle + 1.5 * M_PI) res += (2 * M_PI - currentAngle) + lastAngle; else res += currentAngle - lastAngle; @@ -529,16 +507,15 @@ void TouchInteraction::step(double dt) { dquat camRollRot = angleAxis(_vel.globalRoll*dt, -directionToCenter); globalCamRot = camRollRot * globalCamRot; } - { // Push up to surface + if (!_directTouchMode) { // Push up to surface dvec3 sphereSurfaceToCamera = camPos - (centerPos + centerToBoundingSphere); double distFromSphereSurfaceToCamera = length(sphereSurfaceToCamera); camPos += -directionToCenter * max(minHeightAboveBoundingSphere - distFromSphereSurfaceToCamera, 0.0); - } - - if (!_directTouchMode) - configSensitivities(length(camPos - (centerPos + centerToBoundingSphere))); - decelerate(); + configSensitivities(length(camPos - (centerPos + centerToBoundingSphere))); // might not be needed when direct touch is active + } + + decelerate(); // Update the camera state _camera->setPositionVec3(camPos); _camera->setRotation(globalCamRot * localCamRot); diff --git a/modules/touch/touchmodule.cpp b/modules/touch/touchmodule.cpp index 243084d9df..9df406b9d9 100644 --- a/modules/touch/touchmodule.cpp +++ b/modules/touch/touchmodule.cpp @@ -128,36 +128,11 @@ TouchModule::TouchModule() if (gotNewInput() && OsEng.windowWrapper().isMaster()) { touch->update(list, lastProcessed); - - // for debugging - //std::this_thread::sleep_for(std::chrono::seconds(1)); - std::ostringstream os; - for (const TuioCursor &j : list) { // go through each item - std::list path = j.getPath(); - - TuioTime lastTime = find_if( - lastProcessed.begin(), - lastProcessed.end(), - [&j](const Point& p) { return p.first == j.getSessionID(); } - )->second.getTuioTime(); - - std::list::iterator lastPoint = find_if( - path.begin(), - path.end(), - [&lastTime](const TuioPoint& c) { return lastTime == c.getTuioTime(); }); - - int count = -1; - for (; lastPoint != path.end(); ++lastPoint) // here we can access all elements that are to be processed - count++; - - os << ", Id: " << j.getCursorID() << ", path size: " << j.getPath().size() << ", To Process: " << count; - } - //LINFO("List size: " << list.size() << os.str() << "\n"); - os.clear(); - + //std::this_thread::sleep_for(std::chrono::seconds(1)); // for debugging } - else if (list.size() == 0) + else if (list.size() == 0) { touch->clear(); + } // update lastProcessed lastProcessed.clear();