Add angleaxisfrom2vect()

3 files changed, 70 insertions, 16 deletions

diff --git a/redist/linmath.c b/redist/linmath.c
index b3896ff..5d51708 100644
--- a/redist/linmath.c
+++ b/redist/linmath.c
@@ -104,6 +104,45 @@ void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle)
 	outvec3[2] = w*(u*x + v*y + w*z)*(1-c)   +   z*c   +   (-v*x + u*y)*s;
 }
 
+void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest)
+{
+	FLT v0[3];
+	FLT v1[3];
+	normalize3d(v0, src);
+	normalize3d(v1, dest);
+
+	FLT d = dot3d(v0, v1);// v0.dotProduct(v1);
+
+	// If dot == 1, vectors are the same
+	// If dot == -1, vectors are opposite
+	if (FLT_FABS(d - 1) < DEFAULT_EPSILON)
+	{
+		axis[0] = 0;
+		axis[1] = 1;
+		axis[2] = 0;
+		*angle = 0;
+		return;
+	}
+	else if (FLT_FABS(d + 1) < DEFAULT_EPSILON)
+	{
+		axis[0] = 0;
+		axis[1] = 1;
+		axis[2] = 0;
+		*angle = LINMATHPI;
+		return;
+	}
+
+	FLT v0Len = magnitude3d(v0);
+	FLT v1Len = magnitude3d(v1);
+
+	*angle = FLT_ACOS(d / (v0Len * v1Len));
+
+	//cross3d(c, v0, v1);
+	cross3d(axis, v1, v0);
+
+}
+
+
 /////////////////////////////////////QUATERNIONS//////////////////////////////////////////
 //Originally from Mercury (Copyright (C) 2009 by Joshua Allen, Charles Lohr, Adam Lowman)
 //Under the mit/X11 license.
diff --git a/redist/linmath.h b/redist/linmath.h
index 6f0bf60..8d6cf05 100644
--- a/redist/linmath.h
+++ b/redist/linmath.h
@@ -71,6 +71,8 @@ FLT magnitude3d(const FLT * a );
 FLT anglebetween3d( FLT * a, FLT * b );
 
 void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle);
+void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest);
+
 //Quaternion things...
 
 void quatsetnone( FLT * q );
diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index 481a499..45a1122 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -1471,22 +1471,22 @@ static void QuickPose(SurviveObject *so, int lh)
 			// it basically ignores all the logic to find the most reliable data points
 			// and just grabs a sample and uses it.
 
-			static int countdown = 5;
-
-			if (countdown > 0 && so->ctx->objs[0] == so)
-			{
-				SolveForLighthouse(pos, quat, to, so, 0, lh, 1);
-				countdown--;
-			}
-			else
-			{
-				SolveForLighthouse(pos, quat, to, so, 0, lh, 0);
-			}
+			//static int countdown = 5;
+
+			//if (countdown > 0 && so->ctx->objs[0] == so)
+			//{
+			//	SolveForLighthouse(pos, quat, to, so, 0, lh, 1);
+			//	countdown--;
+			//}
+			//else
+			//{
+			//	SolveForLighthouse(pos, quat, to, so, 0, lh, 0);
+			//}
 			
 
 
 
-			//SolveForLighthouse(pos, quat, to, so, 0, lh, 0);
+			SolveForLighthouse(pos, quat, to, so, 0, lh, 0);
 			printf("!\n");
 		}
 
@@ -1600,6 +1600,11 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 		//quatfrom2vectors(downQuat, negZ, td->down);
 		quatfrom2vectors(downQuat, td->down, negZ);
 
+		FLT angle;
+		FLT axis[3];
+		angleaxisfrom2vect(&angle, &axis, td->down, negZ);
+		//angleaxisfrom2vect(&angle, &axis, negZ, td->down);
+
 		{
 			int sensorCount = 0;
 
@@ -1611,8 +1616,12 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 					FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] };
 					FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] };
 
-					quatrotatevector(norm, downQuat, norm);
-					quatrotatevector(point, downQuat, point);
+					//quatrotatevector(norm, downQuat, norm);
+					//quatrotatevector(point, downQuat, point);
+
+					rotatearoundaxis(norm, norm, axis, angle);
+					rotatearoundaxis(point, point, axis, angle);
+
 
 					to->sensor[sensorCount].normal.x = norm[0];
 					to->sensor[sensorCount].normal.y = norm[1];
@@ -1643,8 +1652,12 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 					FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] };
 					FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] };
 
-					quatrotatevector(norm, downQuat, norm);
-					quatrotatevector(point, downQuat, point);
+					//quatrotatevector(norm, downQuat, norm);
+					//quatrotatevector(point, downQuat, point);
+
+					rotatearoundaxis(norm, norm, axis, angle);
+					rotatearoundaxis(point, point, axis, angle);
+
 
 					to->sensor[sensorCount].normal.x = norm[0];
 					to->sensor[sensorCount].normal.y = norm[1];