From 3635bbcefc81abeb6158f9935480b123c7fa0a6a Mon Sep 17 00:00:00 2001 From: Jonathan Bosson Date: Fri, 14 Apr 2017 12:02:11 -0600 Subject: [PATCH] start testing levmarq use --- modules/touch/ext/levmarq.cpp | 2 +- modules/touch/include/TouchInteraction.h | 2 + modules/touch/src/TouchInteraction.cpp | 112 +++++++++-------------- 3 files changed, 47 insertions(+), 69 deletions(-) diff --git a/modules/touch/ext/levmarq.cpp b/modules/touch/ext/levmarq.cpp index 34c8c0a460..365c2cacbd 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 = 10000; lmstat->init_lambda = 0.0001; lmstat->up_factor = 10; diff --git a/modules/touch/include/TouchInteraction.h b/modules/touch/include/TouchInteraction.h index c82018d438..acd91b1e19 100644 --- a/modules/touch/include/TouchInteraction.h +++ b/modules/touch/include/TouchInteraction.h @@ -87,6 +87,8 @@ struct FunctionData { double aspectRatio; double* measurements; int nDOF; + glm::dvec2(*toScreen)(glm::dvec3, Camera*, SceneGraphNode*, double); + glm::dvec3(*toSurface)(glm::dvec2, Camera*, SceneGraphNode*, double); }; using Point = std::pair; diff --git a/modules/touch/src/TouchInteraction.cpp b/modules/touch/src/TouchInteraction.cpp index 33767c716c..4fec697cc6 100644 --- a/modules/touch/src/TouchInteraction.cpp +++ b/modules/touch/src/TouchInteraction.cpp @@ -85,40 +85,17 @@ void TouchInteraction::update(const std::vector& list, std::vector

measurements[x - 1]; - screenPoint.y = ptr->measurements[x]; - } - else { - screenPoint.x = ptr->measurements[x]; - screenPoint.y = ptr->measurements[x + 1]; - } - - // Find the intersection point in surface coordinates again; - glm::dvec3 camPos = ptr->camera->positionVec3(); - double xCo = 2 * (screenPoint.x - 0.5) * ptr->aspectRatio; - double yCo = -2 * (screenPoint.y - 0.5); // normalized -1 to 1 coordinates on screen - glm::dvec3 raytrace = glm::normalize(ptr->camera->rotationQuaternion() * glm::dvec3(xCo, yCo, -3.2596558)); - glm::dvec3 camToSelectable = ptr->node->worldPosition() - camPos; - double boundingSphere = ptr->node->boundingSphere(); - double d = glm::dot(raytrace, camToSelectable); - double root = boundingSphere * boundingSphere - glm::dot(camToSelectable, camToSelectable) + d * d; - if (root > 0) // two intersection points (take the closest one) - d -= sqrt(root); - glm::dvec3 intersectionPoint = camPos + d * raytrace; - return (glm::inverse(ptr->node->rotationMatrix()) * (intersectionPoint - ptr->node->worldPosition())); - }; - auto toScreen = [](glm::dvec3 vec, FunctionData* ptr) { - glm::dvec3 backToScreenSpace = glm::inverse(ptr->camera->rotationQuaternion()) - * glm::normalize(((ptr->node->rotationMatrix() * vec) + ptr->node->worldPosition() - ptr->camera->positionVec3())); - backToScreenSpace *= (-3.2596558 / backToScreenSpace.z); - return glm::dvec2(backToScreenSpace.x / (2 * ptr->aspectRatio) + 0.5, -backToScreenSpace.y / 2 + 0.5); - }; - FunctionData* ptr = reinterpret_cast(fdata); - glm::dvec3 surfacePoint = toSurface(x, ptr); // get current screen point from the id "x". + glm::dvec2 screenPoint; + if (x % 2) { // true = y[x] is a y-coord, false = y[x] is an x-coord + screenPoint.x = ptr->measurements[x - 1]; + screenPoint.y = ptr->measurements[x]; + } + else { + screenPoint.x = ptr->measurements[x]; + screenPoint.y = ptr->measurements[x + 1]; + } + glm::dvec3 surfacePoint = ptr->toSurface(screenPoint, ptr->camera, ptr->node, ptr->aspectRatio); // get current screen point from the id "x". // Create transformation matrix M(q) and apply transform for newPointInModelView glm::dvec3 T = glm::dvec3(par[0], par[1], 0.0); @@ -136,7 +113,7 @@ void TouchInteraction::update(const std::vector& list, std::vector

toScreen(newSurfacePoint, ptr->camera, ptr->node, ptr->aspectRatio); // go back to screen-space if (x % 2) // return right variable return newScreenPoint.y; @@ -145,7 +122,12 @@ void TouchInteraction::update(const std::vector& list, std::vector

(fdata); + g[0] = 1.0; + g[1] = 1.0; + if (ptr->nDOF > 2) { + g[2] = 1.0; + // Get current screen point from the id "x". glm::dvec2 screenPoint; if (x % 2) { // true = y[x] is a y-coord, false = y[x] is an x-coord screenPoint.x = ptr->measurements[x - 1]; @@ -155,35 +137,7 @@ void TouchInteraction::update(const std::vector& list, std::vector

measurements[x]; screenPoint.y = ptr->measurements[x + 1]; } - - // Find the intersection point in surface coordinates again; - glm::dvec3 camPos = ptr->camera->positionVec3(); - double xCo = 2 * (screenPoint.x - 0.5) * ptr->aspectRatio; - double yCo = -2 * (screenPoint.y - 0.5); // normalized -1 to 1 coordinates on screen - glm::dvec3 raytrace = glm::normalize(ptr->camera->rotationQuaternion() * glm::dvec3(xCo, yCo, -3.2596558)); - glm::dvec3 camToSelectable = ptr->node->worldPosition() - camPos; - double boundingSphere = ptr->node->boundingSphere(); - double d = glm::dot(raytrace, camToSelectable); - double root = boundingSphere * boundingSphere - glm::dot(camToSelectable, camToSelectable) + d * d; - if (root > 0) // two intersection points (take the closest one) - d -= sqrt(root); - glm::dvec3 intersectionPoint = camPos + d * raytrace; - return (glm::inverse(ptr->node->rotationMatrix()) * (intersectionPoint - ptr->node->worldPosition())); - }; - auto toScreen = [](glm::dvec3 vec, FunctionData* ptr) { - glm::dvec3 backToScreenSpace = glm::inverse(ptr->camera->rotationQuaternion()) - * glm::normalize(((ptr->node->rotationMatrix() * vec) + ptr->node->worldPosition() - ptr->camera->positionVec3())); - backToScreenSpace *= (-3.2596558 / backToScreenSpace.z); - return glm::dvec2(backToScreenSpace.x / (2 * ptr->aspectRatio) + 0.5, -backToScreenSpace.y / 2 + 0.5); - }; - - FunctionData* ptr = reinterpret_cast(fdata); - g[0] = 1.0; - g[1] = 1.0; - if (ptr->nDOF > 2) { - g[2] = 1.0; - // Get current screen point from the id "x". - glm::dvec3 surfacePoint = toSurface(x, ptr); + glm::dvec3 surfacePoint = ptr->toSurface(screenPoint, ptr->camera, ptr->node, ptr->aspectRatio); glm::dquat Q; Q.x = par[3]; @@ -201,7 +155,7 @@ void TouchInteraction::update(const std::vector& list, std::vector

toScreen(gradX, ptr->camera, ptr->node, ptr->aspectRatio);; if (x % 2) g[3] = newSPx.y; else @@ -218,8 +172,8 @@ void TouchInteraction::update(const std::vector& list, std::vector

toScreen(gradY, ptr->camera, ptr->node, ptr->aspectRatio);; + glm::dvec2 newSPz = ptr->toScreen(gradZ, ptr->camera, ptr->node, ptr->aspectRatio);; if (x % 2) { g[4] = newSPy.y; @@ -253,7 +207,29 @@ void TouchInteraction::update(const std::vector& list, std::vector

positionVec3(); + double xCo = 2 * (screenPoint.x - 0.5) * aspectRatio; + double yCo = -2 * (screenPoint.y - 0.5); // normalized -1 to 1 coordinates on screen + glm::dvec3 raytrace = glm::normalize(camera->rotationQuaternion() * glm::dvec3(xCo, yCo, -3.2596558)); + glm::dvec3 camToSelectable = node->worldPosition() - camPos; + double boundingSphere = node->boundingSphere(); + double d = glm::dot(raytrace, camToSelectable); + double root = boundingSphere * boundingSphere - glm::dot(camToSelectable, camToSelectable) + d * d; + if (root > 0) // two intersection points (take the closest one) + d -= sqrt(root); + glm::dvec3 intersectionPoint = camPos + d * raytrace; + return (glm::inverse(node->rotationMatrix()) * (intersectionPoint - node->worldPosition())); + }; + auto toScreen = [](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())); + backToScreenSpace *= (-3.2596558 / backToScreenSpace.z); + return glm::dvec2(backToScreenSpace.x / (2 * aspectRatio) + 0.5, -backToScreenSpace.y / 2 + 0.5); + }; + + FunctionData fData = { _camera, _selected.at(0).node, res.x / res.y, screenPoints, nDOF, toScreen, toSurface }; void* dataPtr = reinterpret_cast(&fData); levmarq_init(&_lmstat); bool nIterations = levmarq(nDOF, par, nCoord, screenPoints, squaredError, func, grad, dataPtr, &_lmstat); // finds best transform values and stores them in par