From d71cc918dd2905a74e9ff0b7168433078d598e48 Mon Sep 17 00:00:00 2001
From: Mike Turvey <mturvey6@gmail.com>
Date: Sun, 21 May 2017 16:13:33 -0700
Subject: Fix Tori Poser

quattomatrix33() needed to be updated after the fix in quatfrom2vectors
Additional poser updates to make it more robust.
Poser is now updating based on both lighthouses.  Expect significant jitter from this.
---
 redist/linmath.c       | 32 +++++++++++++++++---------------
 src/poser_turveytori.c | 41 +++++++++++++++++++++++++++++------------
 2 files changed, 46 insertions(+), 27 deletions(-)

diff --git a/redist/linmath.c b/redist/linmath.c
index 5fefe1e..b3896ff 100644
--- a/redist/linmath.c
+++ b/redist/linmath.c
@@ -259,35 +259,37 @@ void quatfrommatrix( FLT * q, const FLT * matrix44 )
 }
 
 
+// Algorithm from http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/
 void quattomatrix33(FLT * matrix33, const FLT * qin)
 {
 	FLT q[4];
 	quatnormalize(q, qin);
 
 	//Reduced calulation for speed
-	FLT xx = 2 * q[0] * q[0];
-	FLT xy = 2 * q[0] * q[1];
-	FLT xz = 2 * q[0] * q[2];
-	FLT xw = 2 * q[0] * q[3];
+	FLT xx = 2 * q[1] * q[1];
+	FLT xy = 2 * q[1] * q[2];
+	FLT xz = 2 * q[1] * q[3];
+	FLT xw = 2 * q[1] * q[0];
+
+	FLT yy = 2 * q[2] * q[2];
+	FLT yz = 2 * q[2] * q[3];
+	FLT yw = 2 * q[2] * q[0];
 
-	FLT yy = 2 * q[1] * q[1];
-	FLT yz = 2 * q[1] * q[2];
-	FLT yw = 2 * q[1] * q[3];
+	FLT zz = 2 * q[3] * q[3];
+	FLT zw = 2 * q[3] * q[0];
 
-	FLT zz = 2 * q[2] * q[2];
-	FLT zw = 2 * q[2] * q[3];
 
 	//opengl major
 	matrix33[0] = 1 - yy - zz;
-	matrix33[1] = xy - zw;
-	matrix33[2] = xz + yw;
+	matrix33[1] = xy + zw;
+	matrix33[2] = xz - yw;
 
-	matrix33[3] = xy + zw;
+	matrix33[3] = xy - zw;
 	matrix33[4] = 1 - xx - zz;
-	matrix33[5] = yz - xw;
+	matrix33[5] = yz + xw;
 
-	matrix33[6] = xz - yw;
-	matrix33[7] = yz + xw;
+	matrix33[6] = xz + yw;
+	matrix33[7] = yz - xw;
 	matrix33[8] = 1 - xx - yy;
 }
 
diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index d5f36cf..0222a46 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -386,13 +386,20 @@ FLT getPointFitnessForPna(Point pointIn, PointsAndAngle *pna)
 	return dist;
 }
 
+int compareFlts(const void * b, const void * a)
+{
+	FLT a2 = *(const FLT*)a;
+	FLT b2 = *(const FLT*)b;
+	return (a2 > b2) - (a2 < b2);
+}
+
 FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount, int deubgPrint)
 {
 	FLT fitness;
 
 	FLT resultSum = 0;
 	FLT *fitnesses = alloca(sizeof(FLT) * pnaCount);
-	int i=0, j=0;
+	int i = 0, j = 0;
 
 	FLT worstFitness = 0;
 
@@ -414,18 +421,18 @@ FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount, int deu
 		}
 	}
 
-	for (size_t i = 0; i < pnaCount; i++)
+
+	qsort(fitnesses, pnaCount, sizeof(FLT), compareFlts);
+
+	//printf("wf[%f]\n", worstFitness);
+
+
+    // Note that we're only using the best 70% of the tori.
+    // This is to remove any "bad" outliers.
+    // TODO: better algorithms exist.
+	for (size_t i = 0; i < (size_t)(pnaCount * 0.70); i++)
 	{
-		// TODO:  This is an UGLY HACK!!!  It is NOT ROBUST and MUST BE BETTER
-		// Right now, we're just throwing away the single worst fitness value
-		// this works frequently, but we REALLY need to do a better job of determing
-		// exactly when we should throw away a bad value.  I'm thinking that decision 
-		// alone could be a master's thesis, so lots of work to be done.
-		// This is just a stupid general approach that helps in a lot of cases,
-		// but is NOT suitable for long term use.
-		//if (pna[i].bi != i && pna[i].bi != j && pna[i].ai != i && pna[i].ai != j)
-		if (fitnesses[i] != worstFitness)
-			resultSum += SQUARED(fitnesses[i]);
+		resultSum += SQUARED(fitnesses[i]);
 	}
 	return 1 / FLT_SQRT(resultSum);
 }
@@ -1523,6 +1530,16 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 				if (counter % 4 == 0)
 					QuickPose(so, 0);
 			}
+			if (1 == l->lh && axis) // only once per full cycle...
+			{
+				static unsigned int counter = 1;
+
+				counter++;
+
+				// let's just do this occasionally for now...
+				if (counter % 4 == 0)
+					QuickPose(so, 1);
+			}
 			// axis changed, time to increment the circular buffer index.
 			td->angleIndex[l->lh][axis]++;
 			td->angleIndex[l->lh][axis] = td->angleIndex[l->lh][axis] % OLD_ANGLES_BUFF_LEN;
-- 
cgit v1.2.3