Use quad intrinsics to compute trochoid normals

Simple algo. Use quad intrinsics to get neighboring pixels' (x & y)
positions in trochoid space. Compute tangent and bitangent from that.
Then normal as cross product. There's some artifacting on diagonal
boundaries.

7 files changed, 339 insertions, 29 deletions

diff --git a/Editor/tooner.cs b/Editor/tooner.cs
index 0d15a3a..310cfa6 100644
--- a/Editor/tooner.cs
+++ b/Editor/tooner.cs
@@ -1990,6 +1990,13 @@ public class ToonerGUI : ShaderGUI {
     bc = FindProperty("_Trochoid_Height_Scale");
     FloatProperty(bc, "Height scale");
 
+    bc = FindProperty("_Trochoid_Enable_Fragment_Normals");
+    enabled = (bc.floatValue != 0.0);
+    EditorGUI.BeginChangeCheck();
+    enabled = Toggle("Enable fragment normals", enabled);
+    EditorGUI.EndChangeCheck();
+    bc.floatValue = enabled ? 1.0f : 0.0f;
+
     EditorGUI.indentLevel -= 1;
   }
 
diff --git a/globals.cginc b/globals.cginc
index 0b1308f..3ed4af2 100644
--- a/globals.cginc
+++ b/globals.cginc
@@ -805,6 +805,7 @@ float _Trochoid_Speed;
 float _Trochoid_Radius_Power;
 float _Trochoid_Radius_Scale;
 float _Trochoid_Height_Scale;
+float _Trochoid_Enable_Fragment_Normals;
 #endif
 
 #if defined(_FACE_ME_WORLD_Y)
diff --git a/math.cginc b/math.cginc
index 5b33edf..507ca74 100644
--- a/math.cginc
+++ b/math.cginc
@@ -249,8 +249,8 @@ bool solveQuadratic(float a, float b, float c, out float x0, out float x1)
 
 float determinant(float3x3 m)
 {
-  return m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1])
-            - m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0])
+  return (m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1])
+            - m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0]))
             + m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);
 }
 
diff --git a/pema_quad_intrinsics.cginc b/pema_quad_intrinsics.cginc
new file mode 100644
index 0000000..0a411a0
--- /dev/null
+++ b/pema_quad_intrinsics.cginc
@@ -0,0 +1,259 @@
+// SPDX-License-Identifier: MIT
+// Author: pema99
+
+// This file contains functions that simulate Quad and Wave Intrinsics without access to either.
+// For more information on those, see: https://github.com/Microsoft/DirectXShaderCompiler/wiki/Wave-Intrinsics
+
+// To use the functions, you must call SETUP_QUAD_INTRINSICS(pos) at the start of your fragment shader,
+// where 'pos' is the pixel position, ie. the fragment input variable with the SV_Position semantic.
+// Note that some functions will require SM 5.0, ie. #pragma target 5.0.
+
+// The file is a bit difficult to read, so here is a quick reference of all the functions it provides:
+//
+// Basic getters:
+//   uint QuadGetLaneID() - Get the ID of the current lane (0-3), from top left to bottom right.
+//   uint2 QuadGetLanePosition() - Get the position of the current lane (0,0 - 1,1), from top left to bottom right.
+//
+// Shuffles and broadcasts:
+//   <float_type> QuadReadAcrossX(<float_type> value) - Read the value of the lane opposite this one on the X axis.
+//   <float_type> QuadReadAcrossY(<float_type> value) - Read the value of the lane opposite this one on the Y axis.
+//   <float_type> QuadReadAcrossDiagonal(<float_type> value) - Read the value of the lane opposite this one on the diagonal.
+//   <float_type> QuadReadLaneAt(<float_type> value, uint2 quadLaneID) - Read the value of the lane at the given position.
+//   <float_type> QuadReadLaneAt(<float_type> value, uint quadLaneID) - Read the value of the lane with the given ID.
+//   void QuadReadAll(<float_type> value, out <float_type> topLeft, out <float_type> topRight, out <float_type> bottomLeft, out <float_type> bottomRight) - Read the value of all lanes.
+//
+// Reductions:
+//   bool QuadAny(bool expr) - Check if any lane evaluate the expression to true.
+//   bool QuadAll(bool expr) - Check if all lanes evaluate the expression to true.
+//   <float_type> QuadSum(<float_type> value) - Sum the values on all lanes.
+//   <float_type> QuadProduct(<float_type> value) - Multiply the values on all lanes.
+//   <float_type> QuadMin(<float_type> value) - Find the minimum value on all lanes.
+//   <float_type> QuadMax(<float_type> value) - Find the maximum value on all lanes.
+//   <integer_type> QuadBitAnd(<integer_type> value) - Bitwise AND the values on all lanes.
+//   <integer_type> QuadBitOr(<integer_type> value) - Bitwise OR the values on all lanes.
+//   <integer_type> QuadBitXor(<integer_type> value) - Bitwise XOR the values on all lanes.
+//   uint4 QuadBallot(bool expr) - Create a bitmask of which lanes evaluate the expression to true.
+//   uint QuadCountBits(bool expr) - Count the number of lanes that evaluate the expression to true.
+//
+// Scans:
+//   <float_type> QuadPrefixSum(<float_type> value) - Sum the values on all lanes up to and exlcuding this one.
+//   <float_type> QuadPrefixProduct(<float_type> value) - Multiply the values on all lanes up to and exlcuding this one.
+//   uint QuadPrefixCountBits(bool expr) - Count the number of lanes that evaluate the expression to true up to and excluding this one.
+
+#ifndef QUAD_INTRINSICS
+#define QUAD_INTRINSICS
+
+// Setup functions
+static uint2 GLOBAL_QUAD_INDEX = uint2(0, 0);
+
+#define SETUP_QUAD_INTRINSICS(SV_Position) \
+    GLOBAL_QUAD_INDEX = (uint2)(SV_Position).xy & 1;
+
+// ID getters
+uint QuadGetLaneID()
+{
+    return ((GLOBAL_QUAD_INDEX.y * 1) << 1) + (GLOBAL_QUAD_INDEX.x & 1);
+}
+
+uint2 QuadGetLanePosition()
+{
+    return GLOBAL_QUAD_INDEX;
+}
+
+// Helper functions
+#define GENERIC_QUAD_FLOAT_HELPERS(T) \
+T QUAD_ADD_HELPER(T a, T b)           \
+{                                     \
+    return a + b;                     \
+}                                     \
+
+// NOTE: The reason we don't implement these for all types is because the HLSL compiler selects
+// overloads based on the size of the type - thus, we can't have any instances that take parameters
+// of the same size, as the overloads will overlap.
+GENERIC_QUAD_FLOAT_HELPERS(float);
+GENERIC_QUAD_FLOAT_HELPERS(float2);
+GENERIC_QUAD_FLOAT_HELPERS(float3);
+GENERIC_QUAD_FLOAT_HELPERS(float4);
+GENERIC_QUAD_FLOAT_HELPERS(float3x3);
+GENERIC_QUAD_FLOAT_HELPERS(float4x4);
+
+#define GENERIC_QUAD_INTEGER_HELPERS(T) \
+T QUAD_BITAND_HELPER(T a, T b)          \
+{                                       \
+    return a & b;                       \
+}                                       \
+                                        \
+T QUAD_BITOR_HELPER(T a, T b)           \
+{                                       \
+    return a | b;                       \
+}                                       \
+                                        \
+T QUAD_BITXOR_HELPER(T a, T b)          \
+{                                       \
+    return a ^ b;                       \
+}
+
+GENERIC_QUAD_INTEGER_HELPERS(uint);
+GENERIC_QUAD_INTEGER_HELPERS(uint2);
+GENERIC_QUAD_INTEGER_HELPERS(uint3);
+GENERIC_QUAD_INTEGER_HELPERS(uint4);
+GENERIC_QUAD_FLOAT_HELPERS(uint3x3);
+GENERIC_QUAD_FLOAT_HELPERS(uint4x4);
+
+uint QUAD_COUNT_BITS_HELPER(uint a, uint b)
+{
+    return a + b;
+}
+
+// Generic intrinsics
+#define GENERIC_QUAD_REDUCTION(T, Name, OP)                                                     \
+T Name(T value)                                                                                 \
+{                                                                                               \
+    T topLeft, topRight, bottomLeft, bottomRight;                                               \
+    QuadReadAll(value, topLeft, topRight, bottomLeft, bottomRight);                             \
+    return OP(OP(OP(topLeft, topRight), bottomLeft), bottomRight);                              \
+}
+
+#define GENERIC_QUAD_SCAN(T, Name, OP)                                                          \
+T Name(T value)                                                                                 \
+{                                                                                               \
+    T topLeft, topRight, bottomLeft, bottomRight;                                               \
+    QuadReadAll(value, topLeft, topRight, bottomLeft, bottomRight);                             \
+    T allValues[4] = { topLeft, topRight, bottomLeft, bottomRight };                            \
+                                                                                                \
+    T prefix = 0;                                                                               \
+    for (int i = 0; i < QuadGetLaneID(); i++)                                                   \
+    {                                                                                           \
+        prefix = OP(prefix, allValues[i]);                                                      \
+    }                                                                                           \
+    return prefix;                                                                              \
+}
+
+#define GENERIC_QUAD_FLOAT_INTRINSICS(T)                                                        \
+T QuadReadAcrossX(T value)                                                                      \
+{                                                                                               \
+    T diff = ddx_fine(value);                                                                   \
+    float sign = GLOBAL_QUAD_INDEX.x == 0 ? 1 : -1;                                             \
+    return (sign * diff) + value;                                                               \
+}                                                                                               \
+                                                                                                \
+T QuadReadAcrossY(T value)                                                                      \
+{                                                                                               \
+    T diff = ddy_fine(value);                                                                   \
+    float sign = GLOBAL_QUAD_INDEX.y == 0 ? 1 : -1;                                             \
+    return (sign * diff) + value;                                                               \
+}                                                                                               \
+                                                                                                \
+T QuadReadAcrossDiagonal(T value)                                                               \
+{                                                                                               \
+    T oppositeX = QuadReadAcrossX(value);                                                       \
+    T oppositeDiagonal = QuadReadAcrossY(oppositeX);                                            \
+    return oppositeDiagonal;                                                                    \
+}                                                                                               \
+                                                                                                \
+T QuadReadLaneAt(T value, uint2 quadLaneID)                                                     \
+{                                                                                               \
+    uint2 offset = 0;                                                                           \
+    bool2 correct = quadLaneID == GLOBAL_QUAD_INDEX;                                            \
+    if (all(correct))                                                                           \
+    {                                                                                           \
+        return value;                                                                           \
+    }                                                                                           \
+    else if (correct.x)                                                                         \
+    {                                                                                           \
+        return QuadReadAcrossY(value);                                                          \
+    }                                                                                           \
+    else if (correct.y)                                                                         \
+    {                                                                                           \
+        return QuadReadAcrossX(value);                                                          \
+    }                                                                                           \
+    else                                                                                        \
+    {                                                                                           \
+        return QuadReadAcrossDiagonal(value);                                                   \
+    }                                                                                           \
+}                                                                                               \
+                                                                                                \
+T QuadReadLaneAt(T value, uint quadLaneID)                                                      \
+{                                                                                               \
+    uint2 offset = 0;                                                                           \
+    return QuadReadLaneAt(value, uint2(quadLaneID & 1, (quadLaneID & 2) >> 1));                 \
+}                                                                                               \
+                                                                                                \
+void QuadReadAll(T value, out T topLeft, out T topRight, out T bottomLeft, out T bottomRight)   \
+{                                                                                               \
+    topLeft = QuadReadLaneAt(value, uint2(0, 0));                                               \
+    topRight = QuadReadLaneAt(value, uint2(1, 0));                                              \
+    bottomLeft = QuadReadLaneAt(value, uint2(0, 1));                                            \
+    bottomRight = QuadReadLaneAt(value, uint2(1, 1));                                           \
+}                                                                                               \
+                                                                                                \
+GENERIC_QUAD_REDUCTION(T, QuadSum, QUAD_ADD_HELPER)                                             \
+GENERIC_QUAD_REDUCTION(T, QuadProduct, mul)                                                     \
+GENERIC_QUAD_REDUCTION(T, QuadMin, min)                                                         \
+GENERIC_QUAD_REDUCTION(T, QuadMax, max)                                                         \
+                                                                                                \
+GENERIC_QUAD_SCAN(T, QuadPrefixSum, QUAD_ADD_HELPER)                                            \
+GENERIC_QUAD_SCAN(T, QuadPrefixProduct, mul)                                                    \
+
+GENERIC_QUAD_FLOAT_INTRINSICS(float);
+GENERIC_QUAD_FLOAT_INTRINSICS(float2);
+GENERIC_QUAD_FLOAT_INTRINSICS(float3);
+GENERIC_QUAD_FLOAT_INTRINSICS(float4);
+GENERIC_QUAD_FLOAT_INTRINSICS(float3x3);
+GENERIC_QUAD_FLOAT_INTRINSICS(float4x4);
+
+// Generic, integer-specific intrincs
+#define GENERIC_QUAD_INTEGER_INTRINSICS(T)                  \
+GENERIC_QUAD_REDUCTION(T, QuadBitAnd, QUAD_BITAND_HELPER)   \
+GENERIC_QUAD_REDUCTION(T, QuadBitOr, QUAD_BITOR_HELPER)     \
+GENERIC_QUAD_REDUCTION(T, QuadBitXor, QUAD_BITXOR_HELPER)
+
+GENERIC_QUAD_INTEGER_INTRINSICS(uint);
+GENERIC_QUAD_INTEGER_INTRINSICS(uint2);
+GENERIC_QUAD_INTEGER_INTRINSICS(uint3);
+GENERIC_QUAD_INTEGER_INTRINSICS(uint4);
+GENERIC_QUAD_INTEGER_INTRINSICS(uint3x3);
+GENERIC_QUAD_INTEGER_INTRINSICS(uint4x4);
+
+// Monomorphic intrinsics
+bool QuadAny(bool expr)
+{
+    return QuadReadLaneAt(expr, 0) || QuadReadLaneAt(expr, 1) || QuadReadLaneAt(expr, 2) || QuadReadLaneAt(expr, 3);
+}
+
+bool QuadAll(bool expr)
+{
+    return QuadReadLaneAt(expr, 0) && QuadReadLaneAt(expr, 1) && QuadReadLaneAt(expr, 2) && QuadReadLaneAt(expr, 3);
+}
+
+uint4 QuadBallot(bool expr)
+{
+    uint4 result;
+    result.x = QuadReadLaneAt(expr ? 1 : 0, 0);
+    result.y = QuadReadLaneAt(expr ? 1 : 0, 1);
+    result.z = QuadReadLaneAt(expr ? 1 : 0, 2);
+    result.w = QuadReadLaneAt(expr ? 1 : 0, 3);
+    return result;
+}
+
+uint QuadCountBits(bool expr)
+{
+    uint4 ballot = QuadBallot(expr);
+    return ballot.x + ballot.y + ballot.z + ballot.w;
+}
+
+GENERIC_QUAD_SCAN(uint, QuadPrefixCountBitsHelper, QUAD_COUNT_BITS_HELPER);
+uint QuadPrefixCountBits(bool expr)
+{
+    return QuadPrefixCountBitsHelper(expr ? 1 : 0);
+}
+
+// Clean up helper macros
+#undef GENERIC_QUAD_INTEGER_HELPERS
+#undef GENERIC_QUAD_FLOAT_HELPERS
+#undef GENERIC_QUAD_REDUCTION
+#undef GENERIC_QUAD_SCAN
+#undef GENERIC_QUAD_FLOAT_INTRINSICS
+#undef GENERIC_QUAD_INTEGER_INTRINSICS
+
+#endif
diff --git a/tooner.shader b/tooner.shader
index b2bdedd..3dad795 100644
--- a/tooner.shader
+++ b/tooner.shader
@@ -853,6 +853,7 @@ Shader "yum_food/tooner"
     _Trochoid_Radius_Power("Radius power", Float) = 1.0
     _Trochoid_Radius_Scale("Radius scale", Float) = 1.0
     _Trochoid_Height_Scale("Height scale", Float) = 1.0
+    _Trochoid_Enable_Fragment_Normals("Enable fragment normals", Float) = 1.0
 
     _FaceMeWorldY_Enable_Static("Enable face me gimmick", Float) = 0.0
     _FaceMeWorldY_Enable_Dynamic("Enable face me gimmick", Float) = 0.0
@@ -1125,6 +1126,7 @@ Shader "yum_food/tooner"
       Tags {
         "LightMode" = "ShadowCaster"
       }
+      Cull [_Cull]
 
       Stencil {
         Ref [_Stencil_Ref_Base]
diff --git a/tooner_lighting.cginc b/tooner_lighting.cginc
index 69b19a4..21741fa 100644
--- a/tooner_lighting.cginc
+++ b/tooner_lighting.cginc
@@ -20,6 +20,7 @@
 #include "motion.cginc"
 #include "oklab.cginc"
 #include "pbr.cginc"
+#include "pema_quad_intrinsics.cginc"
 #include "poi.cginc"
 #include "shear_math.cginc"
 #include "tone.cginc"
@@ -139,17 +140,9 @@ v2f vert(appdata v)
   {
     o.objPos_pre_trochoid = v.vertex.xyz;
 //#define TROCHOID_DECOMPOSE
+#define TROCHOID_SCREEN_SPACE_NORMALS
 #if defined(TROCHOID_DECOMPOSE)
     v.vertex.xyz = cyl2_to_troch_map(cyl_to_cyl2_map(cart_to_cyl_map(v.vertex.xyz)));
-#else
-    v.vertex.xyz = cart_to_troch_map(v.vertex.xyz);
-#endif
-  }
-#endif
-
-#if defined(_TROCHOID)
-  {
-#if defined(TROCHOID_DECOMPOSE)
     // Let h(v) be the trochoid of the cartesian coordinates v.
     // We evaluate h(v) by first mapping it to cylindrical coordinates, then applying a trochoid function defined on those coordinates:
     //   h(v) = h_cyl(g(v))
@@ -167,13 +160,16 @@ v2f vert(appdata v)
     float3x3 j1 = cyl_to_cyl2_jacobian(cart_to_cyl_map(o.objPos_pre_trochoid));
     float3x3 j2 = cyl2_to_troch_jacobian(cyl_to_cyl2_map(cart_to_cyl_map(o.objPos_pre_trochoid)));
     float3x3 vector_mover = transpose(invert(mul(mul(j2, j1), j0)));
+    v.normal = mul(vector_mover, v.normal);
+    v.tangent.xyz = mul(vector_mover, v.tangent.xyz);
 #else
+    v.vertex.xyz = cart_to_troch_map(v.vertex.xyz);
     float3x3 vector_mover = transpose(invert(cart_to_troch_jacobian(o.objPos_pre_trochoid)));
-#endif
     v.normal = mul(vector_mover, v.normal);
     v.tangent.xyz = mul(vector_mover, v.tangent.xyz);
-  }
 #endif
+  }
+#endif  // _TROCHOID
 #if defined(_FACE_ME_WORLD_Y)
   if (_FaceMeWorldY_Enable_Dynamic) {
     // Undo object coordinate system rotation.
@@ -1618,6 +1614,29 @@ float4 effect(inout v2f i, out float depth)
 #else
   depth = 0;
 #endif
+
+#if defined(TROCHOID_SCREEN_SPACE_NORMALS)
+  {
+    float3 my_pos;
+    [branch]
+    if (_Trochoid_Enable_Fragment_Normals) {
+      my_pos = cart_to_troch_map(i.objPos_pre_trochoid.xyz);
+    } else {
+      my_pos = i.objPos.xyz;
+    }
+    float3 neighbor_x = QuadReadAcrossX(my_pos);
+    float3 neighbor_y = QuadReadAcrossY(my_pos);
+    uint2 lane_pos = QuadGetLaneID();
+    float x_sign = (lane_pos == 1 || lane_pos == 3) ? 1 : -1;
+    float y_sign = (lane_pos == 0 || lane_pos == 1) ? 1 : -1;
+    float3 tan1 = (neighbor_x - my_pos) * x_sign;
+    float3 tan2 = (neighbor_y - my_pos) * y_sign;
+    float3 normal = cross(tan1, tan2);
+    i.normal = UnityObjectToWorldNormal(normal);
+    i.tangent.xyz = UnityObjectToWorldDir(tan1);
+  }
+#endif
+
   const float3 view_dir = normalize(_WorldSpaceCameraPos.xyz - i.worldPos);
   const float3 view_dir_c = normalize(i.centerCamPos - i.worldPos);
 #define VIEW_DIR(center_eye_fix) (center_eye_fix == 1 ? view_dir_c : view_dir)
@@ -2908,6 +2927,17 @@ fixed4 frag(v2f i
   UNITY_SETUP_INSTANCE_ID(i);
   UNITY_SETUP_STEREO_EYE_INDEX_POST_VERTEX(i);
 
+  SETUP_QUAD_INTRINSICS(i.pos);
+
+/*
+#if defined(_TROCHOID)
+  float3x3 vector_mover = cyl2_to_troch_jacobian(i.objPos_pre_trochoid);
+  float det = determinant(vector_mover);
+  det = saturate(abs(det));
+  return float4(det, det, det, 1);
+#endif
+*/
+
   return effect(i, depth);
 }
 
diff --git a/trochoid_math.cginc b/trochoid_math.cginc
index 3a5107e..82daf9c 100644
--- a/trochoid_math.cginc
+++ b/trochoid_math.cginc
@@ -17,16 +17,22 @@ float3 cyl2_to_troch_map(float3 v)
   const float d = _Trochoid_d;
   const float rrrr = (R - r) * R / r;
 
+  float rr_gcd = gcd(round(R), round(r));
+  float rr_lcm = (R * r) / rr_gcd;
+  float rr_lcm_factor = rr_lcm / R;
+  float toff = _Time[0] * _Trochoid_Speed;
+
   float x =
-      cos(v.x * R) * v.y * (R - r) * TROCH_POSITION_SCALE +
-      cos(v.x * rrrr) * v.y * d * TROCH_POSITION_SCALE;
+      cos(v.x * rr_lcm_factor * R + toff * 2.3 + toff) * v.y * (R - r) * TROCH_POSITION_SCALE +
+      cos(v.x * rr_lcm_factor * rrrr - toff * 2.9 + toff) * v.y * d * TROCH_POSITION_SCALE;
   float y =
-      sin(v.x * R) * v.y * (R - r) * TROCH_POSITION_SCALE -
-      sin(v.x * rrrr) * v.y * d * TROCH_POSITION_SCALE;
+      sin(v.x * rr_lcm_factor * R - toff * 3.1 + toff) * v.y * (R - r) * TROCH_POSITION_SCALE -
+      sin(v.x * rr_lcm_factor * rrrr + toff * 3.7 + toff) * v.y * d * TROCH_POSITION_SCALE;
   float z =
-      (v.x * v.y * R * TROCH_Z_THETA_SCALE +
-      cos(v.x * R * 5) * v.y * TROCH_POSITION_SCALE +
-      v.y * v.z);
+      (v.x * v.y * rr_lcm_factor * R * TROCH_Z_THETA_SCALE +
+      cos(v.x * rr_lcm_factor * R * 5 + toff * 4.1 + toff) * v.y * TROCH_POSITION_SCALE +
+      //v.y * v.z);
+      v.z);
 
   return float3(x, y, z);
 }
@@ -63,25 +69,30 @@ float3x3 cyl2_to_troch_jacobian(float3 v)
   const float d = _Trochoid_d;
   const float rrrr = (R - r) * R / r;
 
+  float rr_gcd = gcd(round(R), round(r));
+  float rr_lcm = (R * r) / rr_gcd;
+  float rr_lcm_factor = rr_lcm / R;
+  float toff = _Time[0] * _Trochoid_Speed;
+
 #if 1
   float3 df_dt = float3(
-    -R * sin(v.x * R) * v.y * (R - r) * TROCH_POSITION_SCALE +
-    -rrrr * sin(v.x * rrrr) * v.y * d * TROCH_POSITION_SCALE,
+    -R * rr_lcm_factor * sin(v.x * rr_lcm_factor * R + toff * 2.3 + toff) * v.y * (R - r) * TROCH_POSITION_SCALE +
+    -rrrr * rr_lcm_factor * sin(v.x * rr_lcm_factor * rrrr - toff * 2.9 + toff) * v.y * d * TROCH_POSITION_SCALE,
 
-    R * cos(v.x * R) * v.y * (R - r) * TROCH_POSITION_SCALE -
-    rrrr * cos(v.x * rrrr) * v.y * d * TROCH_POSITION_SCALE,
+    R * rr_lcm_factor * cos(v.x * rr_lcm_factor * R - toff * 3.1 + toff) * v.y * (R - r) * TROCH_POSITION_SCALE -
+    rrrr * rr_lcm_factor * cos(v.x * rr_lcm_factor * rrrr + toff * 3.7 + toff) * v.y * d * TROCH_POSITION_SCALE,
 
     v.y * R * TROCH_Z_THETA_SCALE -
-    R * 5 * sin(v.x * R * 5) * v.y * TROCH_POSITION_SCALE);
+    R * rr_lcm_factor *5 * sin(v.x * rr_lcm_factor * R * 5 + toff * 4.1 + toff) * v.y * TROCH_POSITION_SCALE);
 #else
   float3 df_dt = (cyl2_to_troch_map(v + float3(TROCH_EPSILON, 0, 0)) - cyl2_to_troch_map(v - float3(TROCH_EPSILON, 0, 0))) / (2 * TROCH_EPSILON);
 #endif
 
 #if 1
   float3 df_dr = float3(
-    ((R - r) * cos(v.x * R) + d * cos((R - r) * v.x * R / r)) * TROCH_POSITION_SCALE,
-    ((R - r) * sin(v.x * R) - d * sin((R - r) * v.x * R / r)) * TROCH_POSITION_SCALE,
-    cos(v.x * R * 5) * TROCH_POSITION_SCALE);
+    ((R - r) * rr_lcm_factor * cos(v.x * rr_lcm_factor * R + toff * 2.3) + d * rr_lcm_factor * cos((R - r) * v.x * rr_lcm_factor * R / r + toff * 2.9)) * TROCH_POSITION_SCALE,
+    ((R - r) * rr_lcm_factor * sin(v.x * rr_lcm_factor * R + toff * 3.1) - d * rr_lcm_factor * sin((R - r) * v.x * rr_lcm_factor * R / r + toff * 3.7)) * TROCH_POSITION_SCALE,
+    rr_lcm_factor * cos(v.x * rr_lcm_factor * R * 5 + toff * 4.1) * TROCH_POSITION_SCALE);
 #else
   float3 df_dr = (cyl2_to_troch_map(v + float3(0, TROCH_EPSILON, 0)) - cyl2_to_troch_map(v - float3(0, TROCH_EPSILON, 0))) / (2 * TROCH_EPSILON);
 #endif
@@ -90,7 +101,7 @@ float3x3 cyl2_to_troch_jacobian(float3 v)
   float3 df_dz = float3(
     0,
     0,
-    v.y);
+    1);
 #else
   float3 df_dz = (cyl2_to_troch_map(v + float3(0, 0, TROCH_EPSILON)) - cyl2_to_troch_map(v - float3(0, 0, TROCH_EPSILON))) / (2 * TROCH_EPSILON);
 #endif