From f2aebcd880e12653684024010f610a08ec326323 Mon Sep 17 00:00:00 2001
From: Jonathan Bosson <jonathan.bosson@gmail.com>
Date: Wed, 12 Apr 2017 16:54:13 -0600
Subject: [PATCH] one step closer, need to figure out gradient of func and how
 to send cam/node/aspectratio with void* fdata

---
 modules/touch/ext/levmarq.cpp          |  4 +--
 modules/touch/src/TouchInteraction.cpp | 44 ++++++++++++++++++--------
 2 files changed, 33 insertions(+), 15 deletions(-)

diff --git a/modules/touch/ext/levmarq.cpp b/modules/touch/ext/levmarq.cpp
index 349ffe99a4..7b0ff125ad 100644
--- a/modules/touch/ext/levmarq.cpp
+++ b/modules/touch/ext/levmarq.cpp
@@ -163,9 +163,9 @@ int levmarq(int npar, double *par, int ny, double *y, double *dysq,
 double error_func(double *par, int ny, double *y, double *dysq,
 		double (*func)(double *, int, void *), void *fdata) {
 	int x;
-	double res,e=0;
+	double res, e = 0;
 
-	for (x=0; x<ny; x++) {
+	for (x = 0; x < ny; x++) {
 		res = func(par, x, fdata) - y[x];
 		if (dysq) // weighted least-squares 
 			e += res*res/dysq[x];
diff --git a/modules/touch/src/TouchInteraction.cpp b/modules/touch/src/TouchInteraction.cpp
index bc44d1bd93..2a5b16888c 100644
--- a/modules/touch/src/TouchInteraction.cpp
+++ b/modules/touch/src/TouchInteraction.cpp
@@ -81,15 +81,7 @@ TouchInteraction::TouchInteraction()
 TouchInteraction::~TouchInteraction() { }
 
 void TouchInteraction::update(const std::vector<TuioCursor>& list, std::vector<Point>& lastProcessed) {
-	trace(list);
-	if (/*_currentRadius > 0.3 &&*/ _selected.size() == list.size()) { // good value to make any planet sufficiently large for direct-touch, needs better definition
-		_directTouchMode = true;
-	}
-	else {
-		_directTouchMode = false;
-	}
-
-	if (_directTouchMode) {
+	if (_directTouchMode) { // should just be a function call
 		/*
 		1, define s(xi,q): newXi = T(tx,ty,tz)Q(rx,ry,rz)xi, s(xi,q) = modelToScreenSpace(newXi)
 		2, calculate minimum error E = sum( ||s(xi,q)-pi||^2 ) (and define q in the process)
@@ -99,8 +91,27 @@ void TouchInteraction::update(const std::vector<TuioCursor>& list, std::vector<P
 		3, Do the inverse rotation of M(q) on the camera, map interactions to different number of direct touch points
 		*/
 		// define these according to the M(q)
-		auto func = [](double* par, int x, void* fdata) {
-			return par[0] + (par[1] - par[0]) * exp(-par[2] * x);
+		auto func = [](double* par, int x, void* fdata) { // par is the vector of measurements, x current point par[x], fdata additional data needed by the function
+			// define s(xi, q) : newXi = T(tx, ty, tz)Q(rx, ry, rz)xi, s(xi, q) = modelToScreenSpace(newXi)
+			glm::dvec2 screenPoint;
+			if (x % 2) { // true = par[x] is a y-coord, false = par[x] is an x-coord
+				screenPoint.x = par[x-1];
+				screenPoint.y = par[x];
+			}
+			else {
+				screenPoint.x = par[x];
+				screenPoint.y = par[x+1];
+			}
+			// fdata has to contain aspectRatio, camera and node, 
+			//glm::dvec3 modelPoint = screenToModelSpace(screenPoint);
+			// create transformation matrix M(q)
+			//glm::dmat3 T;
+			//glm::dquat Q;
+			glm::dvec2 newScreenPoint; //= modelToScreenSpace(T * (Q * modelPoint));
+			if (x % 2)
+				return newScreenPoint.y;
+			else
+				return newScreenPoint.x;
 		};
 		auto grad = [](double* g, double* par, int x, void* fdata) {
 			g[0] = 1.0 + exp(-par[2] * x);
@@ -119,7 +130,7 @@ void TouchInteraction::update(const std::vector<TuioCursor>& list, std::vector<P
 		double* squaredError = new double[nFingers];
 		int i = 0;
 		for (const SelectedBody& sb : _selected) {
-			glm::dvec2 screenPoint = modelToScreenSpace(sb); // transform to screen-space
+			glm::dvec2 screenPoint = modelToScreenSpace(sb); // transform to screen-space, old point to correct to current
 			contactPoints[i] = screenPoint.x;
 			contactPoints[i + 1] = screenPoint.y;
 
@@ -130,15 +141,22 @@ void TouchInteraction::update(const std::vector<TuioCursor>& list, std::vector<P
 			i += 2;
 		}
 		levmarq_init(&_lmstat);
-		int nIterations = levmarq(nDOF, q, nFingers, contactPoints, squaredError, func, grad, NULL, &_lmstat);
+		int nIterations = levmarq(nDOF, q, nFingers, contactPoints, squaredError, func, grad, NULL, &_lmstat); // NULL should send fdata(camera, node, aspectRatio)
 		delete[] contactPoints;
 		delete[] squaredError;
 	}
 	//else {
+		trace(list);
 		interpret(list, lastProcessed);
 		accelerate(list, lastProcessed);
 	//}
 	
+	if (_currentRadius > 0.3 && _selected.size() == list.size()) { // good value to make any planet sufficiently large for direct-touch, needs better definition
+		_directTouchMode = true;
+	}
+	else {
+		_directTouchMode = false;
+	}
 }
 
 void TouchInteraction::trace(const std::vector<TuioCursor>& list) {