From aca8d20912057d88554d40d24d98a6653b5e2c51 Mon Sep 17 00:00:00 2001
From: Tyler Fox <tyler@blur.com>
Date: Tue, 3 Sep 2024 11:09:12 -0700
Subject: [PATCH] Add an euler filter to the twist spline

---
 src/twistSpline.h | 47 +++++++++++++++++++++++------------------------
 1 file changed, 23 insertions(+), 24 deletions(-)

diff --git a/src/twistSpline.h b/src/twistSpline.h
index 2d7840c..5f90d05 100644
--- a/src/twistSpline.h
+++ b/src/twistSpline.h
@@ -32,7 +32,6 @@ SOFTWARE.
 #include <algorithm>
 #include "twistSplineUtils.h"
 
-
 /**
   * A helper function for quickly finding a single index and segment percentage for an input tValue
   *
@@ -118,12 +117,6 @@ void multiLinearIndexes(const std::vector<Float> &params, const std::vector<Floa
 	}
 }
 
-
-
-
-
-
-
 /**
   * A Spline segment is a 4 vertex cubic bezier spline. Each segment is independent of the others, and
   * segment interdependency will be handled by a setup or rig.
@@ -581,19 +574,18 @@ class TwistSplineSegment {
 	  * linearly interpolating between startAngle and endAngle
 	  */
 	void applyTwist(Float startAngle, Float endAngle){ // inRadians
-        startAngle *= -1;
-        endAngle *= -1;
+		startAngle *= -1;
+		endAngle *= -1;
 
 		resize(tnormals, size(rnormals));
 		resize(tbinormals, size(rbinormals));
 		twistVals.resize(size(rnormals));
 
-
 		Float len = getLength();
 
 		for (IndexType i=0; i <= lutSteps; ++i){
 			Float perc = sampleLengths[i] / len; // parameterize by length
-            Float angle = ((endAngle - startAngle) * perc) + startAngle;
+			Float angle = ((endAngle - startAngle) * perc) + startAngle;
 			twistVals[i] = angle;
 			const Vector &x = rnormals[i];
 			const Vector &y = rbinormals[i];
@@ -612,17 +604,8 @@ class TwistSplineSegment {
 			tbinormals[i] = cross(n, tnormals[i]);
 		}
 	}
-
 };
 
-
-
-
-
-
-
-
-
 // For later:
 // I'll bet I can check the derivatives of the spline
 // to find where it's even possible to be closer, and turn a closest
@@ -665,8 +648,6 @@ class TwistSpline {
 	std::vector<Float> getRemap() const { return remap; }
 	Float getTotalLength() const { return totalLength; }
 
-
-
 	/// Copy constructor
 	TwistSpline(TwistSpline const &old){
 		this->verts = old.verts;
@@ -884,7 +865,6 @@ class TwistSpline {
 		solveTwist();
 	}
 
-
 	/**
 	  * Solve the locks of a parameter of the spline
 	  * Build a tridiagonal matrix that represents each vertex param as a relation to its neighbor params
@@ -973,7 +953,6 @@ class TwistSpline {
 		solveTridiagonalMatrix(mat, res);
 	}
 
-
 	/**
 	  * Solve a tridiagonal matrix in linear time
 	  * If you set up parameter values in a specific way, they can be thought of as a matrix
@@ -1062,6 +1041,26 @@ class TwistSpline {
 		std::vector<Float> oriMap;
 		solveTwistParamMatrix(orientVals, segLens, orientLocks, oriMap);
 
+		// Add an euler filter to the ori map.
+		Float tau = 6.283185307179586477;
+		Float pi =  3.141592653589793238;
+		for (size_t i = 1; i < oriMap.size(); ++i){
+			Float diff = oriMap[i] - oriMap[i - 1];
+			Float sign = 1.0;
+			if (diff < 0.0){
+				diff *= -1.0;
+				sign = -1.0;
+			}
+
+			int count = 0;
+			for (; count < 10; ++count){
+				if (diff < pi){
+					break;
+				}
+				diff -= tau;
+			}
+			oriMap[i] -= count * sign * tau;
+		}
 		std::vector<Float> twistMap;
 		solveTwistParamMatrix(userTwists, segLens, twistLocks, twistMap);