From 0542e7cef57dfb3f7a07a765fcbf98086541effb Mon Sep 17 00:00:00 2001
From: Jonathan Bosson <jonathan.bosson@gmail.com>
Date: Thu, 20 Apr 2017 15:22:53 -0600
Subject: [PATCH] Levmarq gives right q values, need to transform them to
 camera space correctly

---
 ext/ghoul                              |  2 +-
 modules/touch/ext/levmarq.cpp          |  4 +--
 modules/touch/src/TouchInteraction.cpp | 50 ++++++++++++--------------
 openspace.cfg                          |  2 +-
 4 files changed, 26 insertions(+), 32 deletions(-)

diff --git a/ext/ghoul b/ext/ghoul
index c37ff8beb5..200401261c 160000
--- a/ext/ghoul
+++ b/ext/ghoul
@@ -1 +1 @@
-Subproject commit c37ff8beb5b8825ecb67b121ae4deac34677e1bd
+Subproject commit 200401261cba513fb1faa4b7e92bd213435625ad
diff --git a/modules/touch/ext/levmarq.cpp b/modules/touch/ext/levmarq.cpp
index b3c472f89c..6f690930dc 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 = 1;
+	lmstat->verbose = 0;
 	lmstat->max_it = 5000;
 	lmstat->init_lambda = 1e-6;
 	lmstat->up_factor = 10;
@@ -102,7 +102,7 @@ int levmarq(int npar, double *par, int ny, double* y, double *dysq,
 				weight = 1/dysq[x]; // for weighted least-squares
 			grad(g, par, x, fdata);
 			for (i = 0; i < npar; i++) {
-				d[i] += (y[i] - func(par, x, fdata)) * g[i] * weight; //(y[x] - func(par, x, fdata)) * g[i] * weight;
+				d[i] += (0.0 - func(par, x, fdata)) * g[i] * weight; //(y[x] - func(par, x, fdata)) * g[i] * weight;
 				for (j = 0; j <= i; j++)
 					h[i][j] += g[i] * g[j] * weight;
 			}
diff --git a/modules/touch/src/TouchInteraction.cpp b/modules/touch/src/TouchInteraction.cpp
index f328aa0305..cd4f91a188 100644
--- a/modules/touch/src/TouchInteraction.cpp
+++ b/modules/touch/src/TouchInteraction.cpp
@@ -101,7 +101,11 @@ void TouchInteraction::update(const std::vector<TuioCursor>& list, std::vector<P
 			}
 			Q.w = sqrt(1.0 - std::min(glm::length(Q), 0.99999)); // if check is not done Q.w becomes NAN. Do we need a better definition of Q.w?
 			
-			glm::dvec3 newSurfacePoint = (Q * selectedPoint) + T;
+			glm::dmat4 rot = glm::toMat4(Q);
+			glm::dmat4 trans = glm::translate(glm::dmat4(1.0f), T);
+			glm::dmat4 final = rot * trans;
+
+			glm::dvec3 newSurfacePoint = final * glm::dvec4(selectedPoint, 1.0); // maybe opposite, maybe use Quat as par 0-2
 			glm::dvec2 newScreenPoint = ptr->toScreen(newSurfacePoint, ptr->camera, ptr->node, ptr->aspectRatio);
 
 			return glm::length(ptr->screenPoints.at(x) - newScreenPoint);
@@ -127,7 +131,7 @@ void TouchInteraction::update(const std::vector<TuioCursor>& list, std::vector<P
 
 		SceneGraphNode* node = _selected.at(0).node;
 		const int nFingers = list.size();
-		int nDOF = std::min(static_cast<int>(list.size() * 2), 6);
+		int nDOF = std::min(nFingers * 2, 6);
 		double* par = new double[nDOF];
 		double tPar[6] = { node->worldPosition().x, node->worldPosition().y, node->worldPosition().z, 0.0, 0.0, 0.0 };
 		for (int i = 0; i < nDOF; ++i) // initial values of q or 0.0? (ie current model or no rotation/translation)
@@ -146,23 +150,6 @@ void TouchInteraction::update(const std::vector<TuioCursor>& list, std::vector<P
 			squaredError[i] = err;
 		}
 		
-
-		auto toSurface = [](glm::dvec2 screenPoint, Camera* camera, SceneGraphNode* node, double aspectRatio) {
-			// Find the intersection point in surface coordinates again;
-			glm::dvec3 camPos = camera->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()));
@@ -171,7 +158,7 @@ void TouchInteraction::update(const std::vector<TuioCursor>& list, std::vector<P
 		};
 
 		glm::dvec2 res = OsEng.windowWrapper().currentWindowResolution();
-		FunctionData fData = { selectedPoints, screenPoints, nDOF, toScreen, distToMinimize, _camera, node, res.x / res.y};
+		FunctionData fData = { selectedPoints, screenPoints, nDOF, toScreen, distToMinimize, _camera, node, res.x / res.y };
 		void* dataPtr = reinterpret_cast<void*>(&fData);
 
 		levmarq_init(&_lmstat);
@@ -187,15 +174,22 @@ void TouchInteraction::update(const std::vector<TuioCursor>& list, std::vector<P
 		double len = Q.x*Q.x + Q.y*Q.y + Q.z*Q.z;
 		Q.w = sqrt(1.0 - len);
 
-		// debugging
-		std::ostringstream os;
-		for (int i = 0; i < nDOF; ++i) {
-			os << par[i] << ", ";
-		}
-		std::cout << "\nLevmarq success after " << nIterations << " iterations, Print par[nDOF]: " << os.str() << "\n";
 
-		_camera->setPositionVec3(_camera->positionVec3() - T);
-		//_camera->rotate(glm::inverse(Q));
+		glm::dmat4 rot = glm::toMat4(Q);
+		glm::dmat4 trans = glm::translate(glm::dmat4(1.0f), T);
+		glm::dmat4 finalTransform = (static_cast<glm::dmat4>(node->rotationMatrix()) * (rot * trans));
+
+
+		// newWorldPoint = node->rotationMatrix() * ((M(q) * selectedPoint)) (+ node->worldPosition())
+
+		glm::dquat camQ = glm::inverse(Q);
+		glm::dvec3 camT = glm::dvec3(finalTransform[3][0], finalTransform[3][1], finalTransform[3][2]); //node->rotationMatrix() * T;
+
+		//_camera->rotate(camQ);
+		_camera->setPositionVec3(_camera->positionVec3() - camT);
+
+		// debugging
+		std::cout << "Levmarq success after " << nIterations << " iterations. Camera T: " << glm::to_string(camT) << ", Q: " << glm::to_string(camQ) << ", " << glm::to_string(finalTransform) << "\n";
 
 		// cleanup
 		delete[] squaredError;
diff --git a/openspace.cfg b/openspace.cfg
index c4626ffadb..ceceb5af30 100644
--- a/openspace.cfg
+++ b/openspace.cfg
@@ -26,7 +26,7 @@ return {
 
     -- Sets the scene that is to be loaded by OpenSpace. A scene file is a description
     -- of all entities that will be visible during an instance of OpenSpace
-    Scene = "${SCENE}/default.scene",
+    Scene = "${SCENE}/globebrowsing.scene",
     -- Scene = "${SCENE}/globebrowsing.scene",
     -- Scene = "${SCENE}/rosetta.scene",
     -- Scene = "${SCENE}/dawn.scene",