using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Reflection;
using System.Runtime.CompilerServices;
using System.Runtime.Versioning;
using BepInEx;
using BepInEx.Bootstrap;
using BepInEx.Configuration;
using BepInEx.Logging;
using HarmonyLib;
using LocalValheimFixes;
using Microsoft.CodeAnalysis;
using UniGLTF;
using UniGLTF.Utils;
using UniVRM10.FastSpringBones.System;
using UnityEngine;

[assembly: CompilationRelaxations(8)]
[assembly: RuntimeCompatibility(WrapNonExceptionThrows = true)]
[assembly: Debuggable(DebuggableAttribute.DebuggingModes.IgnoreSymbolStoreSequencePoints)]
[assembly: TargetFramework(".NETStandard,Version=v2.1", FrameworkDisplayName = ".NET Standard 2.1")]
[assembly: AssemblyCompany("NordicGlutePhysics")]
[assembly: AssemblyConfiguration("Release")]
[assembly: AssemblyFileVersion("0.1.0.0")]
[assembly: AssemblyInformationalVersion("0.1.0+3356aed1f6508684669959e6a326f48d5ef50221")]
[assembly: AssemblyProduct("NordicGlutePhysics")]
[assembly: AssemblyTitle("NordicGlutePhysics")]
[assembly: AssemblyVersion("0.1.0.0")]
[module: RefSafetyRules(11)]
namespace Microsoft.CodeAnalysis
{
	[CompilerGenerated]
	[Embedded]
	internal sealed class EmbeddedAttribute : Attribute
	{
	}
}
namespace System.Runtime.CompilerServices
{
	[CompilerGenerated]
	[Embedded]
	[AttributeUsage(AttributeTargets.Module, AllowMultiple = false, Inherited = false)]
	internal sealed class RefSafetyRulesAttribute : Attribute
	{
		public readonly int Version;

		public RefSafetyRulesAttribute(int P_0)
		{
			Version = P_0;
		}
	}
}
namespace NordicGlutePhysics
{
	public struct GluteFrame
	{
		public D3 P;

		public M3 R;

		public double S;

		public GluteFrame(D3 p, M3 r, double s)
		{
			P = p;
			R = r;
			S = s;
		}
	}
	public sealed class GluteParams
	{
		public double[] Freq;

		public double[] Zeta;

		public double[] LimPos;

		public double[] LimNeg;

		public double Inertia = 1.0;

		public double Twist = 2.0;

		public double HpHz = 0.8;

		public double Steady = 0.2;

		public double EnvK = 1.5;

		public double EnvAttack = 0.25;

		public double EnvRelease = 1.5;

		public double DriveHz = 3.0;

		public double Gravity = 0.1;

		public double Amount = 1.0;

		public double ThighDrive = 0.3;

		public double PivotUp = 0.06;

		public double PivotIn = 0.08;

		public double RotShare = 1.0;

		public double MaxTurnDeg = 25.0;

		public double FilterHz = 12.0;

		public double MaxAccel = 60.0;

		public double MaxSpeed = 1.2;

		public double Substep = 1.0 / 240.0;

		public double SoftStart = 0.7;

		public double SoftFreq = 5.0;

		public double LimZeta = 0.9;

		public double HardAt = 1.3;

		public GluteParams Clone()
		{
			GluteParams obj = (GluteParams)MemberwiseClone();
			obj.Freq = (double[])Freq.Clone();
			obj.Zeta = (double[])Zeta.Clone();
			obj.LimPos = (double[])LimPos.Clone();
			obj.LimNeg = (double[])LimNeg.Clone();
			return obj;
		}
	}
	public static class GlutePresets
	{
		public static readonly string[] Names = new string[3] { "light", "visible", "strong" };

		public static GluteParams For(string name)
		{
			string text = (name ?? "").ToLowerInvariant();
			if (!(text == "light"))
			{
				if (text == "strong")
				{
					return Make(new double[3] { 2.0, 1.5, 2.1 }, new double[3] { 0.8, 0.8, 0.8 }, new double[6] { 0.026, 0.03, 0.03, 0.022, 0.036, 0.022 }, 1.1, 0.3);
				}
				return Make(new double[3] { 2.2, 1.6, 2.3 }, new double[3] { 0.85, 0.85, 0.85 }, new double[6] { 0.022, 0.026, 0.026, 0.018, 0.03, 0.018 }, 0.9, 0.3);
			}
			return Make(new double[3] { 2.4, 1.8, 2.5 }, new double[3] { 0.9, 0.9, 0.9 }, new double[6] { 0.018, 0.02, 0.02, 0.014, 0.024, 0.014 }, 0.65, 0.3);
		}

		private static GluteParams Make(double[] freq, double[] zeta, double[] lim, double inertia, double steady)
		{
			GluteParams gluteParams = new GluteParams();
			gluteParams.Freq = freq;
			gluteParams.Zeta = zeta;
			gluteParams.LimPos = new double[3]
			{
				lim[0],
				lim[1],
				lim[2]
			};
			gluteParams.LimNeg = new double[3]
			{
				lim[3],
				lim[4],
				lim[5]
			};
			gluteParams.Inertia = inertia;
			gluteParams.Steady = steady;
			return gluteParams;
		}
	}
	public sealed class GluteSide
	{
		private struct Prev
		{
			public D3 A;

			public D3 P;

			public D3 V;

			public D3 W;

			public M3 M;

			public int N;

			public double Dt;
		}

		public const double Gravity = 9.81;

		public readonly D3 HeadHip;

		public readonly D3 HeadThigh;

		public readonly M3 Basis;

		public readonly double Hand;

		public GluteParams Params;

		public D3 X;

		public D3 V;

		public D3 Lp;

		public D3 Env;

		public D3 Drive;

		public D3 F1;

		public D3 F2;

		private Prev prev;

		private bool hasPrev;

		public GluteSide(D3 headHip, D3 headThigh, M3 hipRot, D3 headWorld, D3 midWorld, GluteParams p)
		{
			HeadHip = headHip;
			HeadThigh = headThigh;
			D3 d = MulMtV(hipRot, new D3(0.0, 1.0, 0.0));
			D3 d2 = MulMtV(hipRot, new D3(0.0, 0.0, -1.0));
			d2 = Norm(d2 - d * Dot(d2, d));
			D3 d3 = Cross(d, d2);
			if (Dot(MulMV(hipRot, d3), headWorld - midWorld) < 0.0)
			{
				d3 *= -1.0;
			}
			Basis = new M3(d3, d, d2);
			Hand = ((!(Dot(Cross(d3, d), d2) < 0.0)) ? 1 : (-1));
			Params = p;
			Reset();
		}

		public void Reset()
		{
			X = default(D3);
			V = default(D3);
			Lp = default(D3);
			Env = default(D3);
			Drive = default(D3);
			F1 = default(D3);
			F2 = default(D3);
			hasPrev = false;
		}

		private D3 Anchor(GluteFrame hip, GluteFrame thigh)
		{
			D3 d = hip.P + MulMV(hip.R, HeadHip) * hip.S;
			double thighDrive = Params.ThighDrive;
			if (thighDrive == 0.0)
			{
				return d;
			}
			D3 d2 = thigh.P + MulMV(thigh.R, HeadThigh) * thigh.S;
			return d + (d2 - d) * thighDrive;
		}

		public void Step(double dt, GluteFrame hip, GluteFrame thigh)
		{
			GluteParams gluteParams = Params;
			double s = hip.S;
			D3 d = Anchor(hip, thigh);
			M3 m = MulMM(hip.R, Basis);
			if (!(dt > 0.0))
			{
				return;
			}
			double num = 0.0;
			if (hasPrev)
			{
				double num2 = 0.0;
				for (int i = 0; i < 3; i++)
				{
					num2 += Dot(prev.M.Col(i), m.Col(i));
				}
				num = Math.Acos(Math.Max(-1.0, Math.Min(1.0, (num2 - 1.0) / 2.0)));
			}
			if (!hasPrev || dt > 0.1 || Len(d - prev.A) > 1.0 || num > Math.PI / 2.0)
			{
				prev = new Prev
				{
					A = d,
					M = m,
					P = hip.P,
					V = default(D3),
					W = default(D3),
					N = 0,
					Dt = dt
				};
				hasPrev = true;
				return;
			}
			D3 d2 = new D3(0.0, 0.0, 0.0);
			for (int j = 0; j < 3; j++)
			{
				d2 += Cross(prev.M.Col(j), m.Col(j));
			}
			double num3 = Len(d2);
			d2 = ((num3 > 1E-12) ? (d2 * (num / (dt * num3))) : new D3(0.0, 0.0, 0.0));
			D3 d3 = (d - prev.A) * (1.0 / dt);
			double t = ((gluteParams.FilterHz > 0.0) ? (1.0 - Math.Exp(Math.PI * -2.0 * gluteParams.FilterHz * dt)) : 1.0);
			bool num4 = prev.N == 0;
			D3 d4 = (num4 ? d3 : Lerp(prev.V, d3, t));
			D3 d5 = (num4 ? d2 : Lerp(prev.W, d2, t));
			double num5 = (prev.Dt + dt) / 2.0;
			D3 d6 = (num4 ? new D3(0.0, 0.0, 0.0) : ((d4 - prev.V) * (1.0 / num5)));
			D3 a = (num4 ? new D3(0.0, 0.0, 0.0) : ((d5 - prev.W) * (1.0 / num5)));
			d6 += Cross(a, prev.A - prev.P) * (gluteParams.Twist - 1.0);
			d6 *= 1.0 / Math.Sqrt(1.0 + Dot(d6, d6) / (gluteParams.MaxAccel * gluteParams.MaxAccel));
			D3 d7 = MulMtV(prev.M, d6);
			double t2 = ((gluteParams.HpHz > 0.0) ? (1.0 - Math.Exp(Math.PI * -2.0 * gluteParams.HpHz * dt)) : 0.0);
			Lp = Lerp(Lp, d7, t2);
			D3 d8 = d7 - Lp;
			D3 d9 = new D3(0.0, 0.0, 0.0);
			double num6 = 1.0 - Math.Exp((0.0 - dt) / gluteParams.EnvAttack);
			double num7 = 1.0 - Math.Exp((0.0 - dt) / gluteParams.EnvRelease);
			for (int k = 0; k < 3; k++)
			{
				double num8 = Math.Abs(d8[k]);
				double num9 = gluteParams.EnvK * Env[k];
				double num10 = Math.Max(0.0, num8 - num9);
				double num11 = Math.Min(num8, num9);
				d9[k] = (double)Math.Sign(d8[k]) * (num10 + gluteParams.Steady * num11);
				Env[k] += (num8 - Env[k]) * ((num8 > Env[k]) ? num6 : num7);
			}
			D3 f = F2;
			if (gluteParams.DriveHz > 0.0)
			{
				double t3 = 1.0 - Math.Exp(Math.PI * -2.0 * gluteParams.DriveHz * dt);
				F1 = Lerp(F1, d9, t3);
				F2 = Lerp(F2, F1, t3);
			}
			else
			{
				F1 = d9;
				F2 = d9;
			}
			D3 b = (Drive = F2);
			D3 a2 = MulMtV(prev.M, new D3(0.0, -9.81, 0.0));
			D3 b2 = MulMtV(m, new D3(0.0, -9.81, 0.0));
			int num12 = Math.Max(1, (int)Math.Ceiling(dt / gluteParams.Substep - 1E-09));
			double num13 = dt / (double)num12;
			for (int l = 1; l <= num12; l++)
			{
				D3 d10 = Lerp(a2, b2, (double)l / (double)num12);
				D3 d11 = Lerp(f, b, (double)l / (double)num12);
				D3 a3 = new D3(0.0, 0.0, 0.0);
				for (int n = 0; n < 3; n++)
				{
					double num14 = (0.0 - gluteParams.Inertia) * d11[n] + gluteParams.Gravity * (d10[n] - ((n == 1) ? (-9.81) : 0.0));
					double num15 = Math.PI * 2.0 * gluteParams.Freq[n];
					a3[n] = num14 - num15 * num15 * X[n] - 2.0 * gluteParams.Zeta[n] * num15 * V[n];
				}
				SoftLimit(ref a3, X, V, gluteParams, s);
				for (int num16 = 0; num16 < 3; num16++)
				{
					V[num16] += a3[num16] * num13;
				}
				double num17 = Len(V);
				double num18 = gluteParams.MaxSpeed * s;
				if (num17 > num18)
				{
					V *= num18 / num17;
				}
				for (int num19 = 0; num19 < 3; num19++)
				{
					X[num19] += V[num19] * num13;
				}
				HardLimit(ref X, ref V, gluteParams, s);
			}
			prev = new Prev
			{
				A = d,
				M = m,
				P = hip.P,
				V = d4,
				W = d5,
				N = prev.N + 1,
				Dt = dt
			};
		}

		public void Motion(double hipScale, out Q4 Q, out D3 d)
		{
			GluteParams gluteParams = Params;
			M3 basis = Basis;
			d = MulMV(basis, X) * (gluteParams.Amount / hipScale);
			D3 d2 = (basis.C1 * (0.0 - gluteParams.PivotUp) + basis.C2 * gluteParams.PivotIn) * (1.0 / hipScale);
			double num = Len(d2);
			D3 d3 = d2 * (1.0 / num);
			D3 d4 = d * gluteParams.RotShare;
			d4 -= d3 * Dot(d4, d3);
			double num2 = num * Math.Tan(gluteParams.MaxTurnDeg * Math.PI / 180.0);
			double num3 = Len(d4);
			if (num3 > 1E-12)
			{
				d4 *= num2 * Math.Tanh(num3 / num2) / num3;
			}
			Q = QFromTo(d3, Norm(d2 + d4));
		}

		public void Pose(GluteFrame hip, GluteFrame thigh, out D3 hipPos, out Q4 hipRot, out D3 thighPos, out Q4 thighRot)
		{
			Motion(hip.S, out var Q, out var d);
			hipPos = HeadHip + d;
			hipRot = Q;
			D3 d2 = thigh.P + MulMV(thigh.R, HeadThigh) * thigh.S;
			D3 d3 = MulMtV(hip.R, d2 - hip.P) * (1.0 / hip.S);
			D3 v = QRotate(Q, d3 - HeadHip) + HeadHip + d;
			D3 d4 = hip.P + MulMV(hip.R, v) * hip.S;
			thighPos = MulMtV(thigh.R, d4 - thigh.P) * (1.0 / thigh.S);
			Q4 q = QuatFromMat(hip.R);
			Q4 q2 = QuatFromMat(thigh.R);
			thighRot = QMul(QMul(QConj(q2), QMul(QMul(q, Q), QConj(q))), q2);
		}

		private static void Ellipsoid(D3 x, GluteParams p, double s, out double r, out D3 L)
		{
			double num = 0.0;
			L = new D3(0.0, 0.0, 0.0);
			for (int i = 0; i < 3; i++)
			{
				L[i] = ((x[i] >= 0.0) ? p.LimPos[i] : p.LimNeg[i]) * s;
				num += x[i] / L[i] * (x[i] / L[i]);
			}
			r = Math.Sqrt(num);
		}

		private static void SoftLimit(ref D3 a, D3 x, D3 v, GluteParams p, double s)
		{
			Ellipsoid(x, p, s, out var r, out var L);
			if (!(r <= p.SoftStart))
			{
				double num = (r - p.SoftStart) / (1.0 - p.SoftStart);
				double num2 = Math.PI * 2.0 * p.SoftFreq;
				double num3 = num2 * num2 * num * num;
				D3 b = Norm(new D3(x.X / (L.X * L.X), x.Y / (L.Y * L.Y), x.Z / (L.Z * L.Z)));
				double num4 = Dot(x, b);
				double num5 = 2.0 * p.LimZeta * num2 * num;
				double num6 = Dot(v, b);
				for (int i = 0; i < 3; i++)
				{
					a[i] -= (num3 * num4 + num5 * num6) * b[i];
				}
			}
		}

		private static void HardLimit(ref D3 x, ref D3 v, GluteParams p, double s)
		{
			Ellipsoid(x, p, s, out var r, out var L);
			if (r <= p.HardAt)
			{
				return;
			}
			for (int i = 0; i < 3; i++)
			{
				x[i] *= p.HardAt / r;
			}
			D3 b = Norm(new D3(x.X / (L.X * L.X), x.Y / (L.Y * L.Y), x.Z / (L.Z * L.Z)));
			double num = Dot(v, b);
			if (num > 0.0)
			{
				for (int j = 0; j < 3; j++)
				{
					v[j] -= num * b[j];
				}
			}
		}

		private static double Dot(D3 a, D3 b)
		{
			return a.X * b.X + a.Y * b.Y + a.Z * b.Z;
		}

		private static D3 Cross(D3 a, D3 b)
		{
			return new D3(a.Y * b.Z - a.Z * b.Y, a.Z * b.X - a.X * b.Z, a.X * b.Y - a.Y * b.X);
		}

		private static double Len(D3 a)
		{
			return Math.Sqrt(Dot(a, a));
		}

		private static D3 Norm(D3 a)
		{
			double num = Len(a);
			if (!(num > 1E-12))
			{
				return new D3(0.0, 0.0, 0.0);
			}
			return a * (1.0 / num);
		}

		private static D3 Lerp(D3 a, D3 b, double t)
		{
			return new D3(a.X + (b.X - a.X) * t, a.Y + (b.Y - a.Y) * t, a.Z + (b.Z - a.Z) * t);
		}

		private static D3 MulMV(M3 m, D3 v)
		{
			return new D3(m.C0.X * v.X + m.C1.X * v.Y + m.C2.X * v.Z, m.C0.Y * v.X + m.C1.Y * v.Y + m.C2.Y * v.Z, m.C0.Z * v.X + m.C1.Z * v.Y + m.C2.Z * v.Z);
		}

		private static D3 MulMtV(M3 m, D3 v)
		{
			return new D3(m.C0.X * v.X + m.C0.Y * v.Y + m.C0.Z * v.Z, m.C1.X * v.X + m.C1.Y * v.Y + m.C1.Z * v.Z, m.C2.X * v.X + m.C2.Y * v.Y + m.C2.Z * v.Z);
		}

		private static M3 MulMM(M3 a, M3 b)
		{
			return new M3(MulMV(a, b.C0), MulMV(a, b.C1), MulMV(a, b.C2));
		}

		private static Q4 QMul(Q4 q, Q4 r)
		{
			return new Q4(q.W * r.X + q.X * r.W + q.Y * r.Z - q.Z * r.Y, q.W * r.Y - q.X * r.Z + q.Y * r.W + q.Z * r.X, q.W * r.Z + q.X * r.Y - q.Y * r.X + q.Z * r.W, q.W * r.W - q.X * r.X - q.Y * r.Y - q.Z * r.Z);
		}

		private static Q4 QConj(Q4 q)
		{
			return new Q4(0.0 - q.X, 0.0 - q.Y, 0.0 - q.Z, q.W);
		}

		private static D3 QRotate(Q4 q, D3 v)
		{
			D3 a = new D3(q.X, q.Y, q.Z);
			D3 d = Cross(a, v) * 2.0;
			return v + d * q.W + Cross(a, d);
		}

		private static Q4 QFromTo(D3 a, D3 b)
		{
			D3 d = Cross(a, b);
			double num = 1.0 + Dot(a, b);
			if (num < 1E-08)
			{
				return new Q4(0.0, 0.0, 0.0, 1.0);
			}
			double num2 = Math.Sqrt(d.X * d.X + d.Y * d.Y + d.Z * d.Z + num * num);
			return new Q4(d.X / num2, d.Y / num2, d.Z / num2, num / num2);
		}

		public static Q4 QuatFromMat(M3 m)
		{
			double x = m.C0.X;
			double y = m.C0.Y;
			double z = m.C0.Z;
			double x2 = m.C1.X;
			double y2 = m.C1.Y;
			double z2 = m.C1.Z;
			double x3 = m.C2.X;
			double y3 = m.C2.Y;
			double z3 = m.C2.Z;
			double num = x + y2 + z3;
			Q4 q;
			if (num > 0.0)
			{
				double num2 = Math.Sqrt(num + 1.0) * 2.0;
				q = new Q4((z2 - y3) / num2, (x3 - z) / num2, (y - x2) / num2, 0.25 * num2);
			}
			else if (x > y2 && x > z3)
			{
				double num3 = Math.Sqrt(1.0 + x - y2 - z3) * 2.0;
				q = new Q4(0.25 * num3, (x2 + y) / num3, (x3 + z) / num3, (z2 - y3) / num3);
			}
			else if (y2 > z3)
			{
				double num4 = Math.Sqrt(1.0 + y2 - x - z3) * 2.0;
				q = new Q4((x2 + y) / num4, 0.25 * num4, (y3 + z2) / num4, (x3 - z) / num4);
			}
			else
			{
				double num5 = Math.Sqrt(1.0 + z3 - x - y2) * 2.0;
				q = new Q4((x3 + z) / num5, (y3 + z2) / num5, 0.25 * num5, (y - x2) / num5);
			}
			double num6 = Math.Sqrt(q.X * q.X + q.Y * q.Y + q.Z * q.Z + q.W * q.W);
			return new Q4(q.X / num6, q.Y / num6, q.Z / num6, q.W / num6);
		}
	}
	[BepInPlugin("local.valheim.nordicglutephysics", "Nordic Glute Physics (prototype)", "0.3.0")]
	[BepInDependency("com.rawrtastic.plugins.enhancedvalheimvrm", "1.0.0")]
	public sealed class GlutePhysicsPlugin : BaseUnityPlugin
	{
		internal static ManualLogSource Log;

		internal static ConfigEntry<bool> Enabled;

		internal static ConfigEntry<string> Preset;

		internal static ConfigEntry<float> Amount;

		private static FieldInfo cloneField;

		internal static readonly List<GluteJiggleDriver> Drivers = new List<GluteJiggleDriver>();

		internal static int ServiceFrame = -1;

		private void Awake()
		{
			//IL_0054: Unknown result type (might be due to invalid IL or missing references)
			//IL_005e: Expected O, but got Unknown
			//IL_0091: Unknown result type (might be due to invalid IL or missing references)
			//IL_009b: Expected O, but got Unknown
			//IL_0130: Unknown result type (might be due to invalid IL or missing references)
			//IL_0136: Expected O, but got Unknown
			//IL_0148: Unknown result type (might be due to invalid IL or missing references)
			//IL_0156: Expected O, but got Unknown
			//IL_0199: Unknown result type (might be due to invalid IL or missing references)
			//IL_01a6: Expected O, but got Unknown
			Log = ((BaseUnityPlugin)this).Logger;
			Enabled = ((BaseUnityPlugin)this).Config.Bind<bool>("GlutePhysics", "Enabled", true, "Glute physics for avatars that have the Glute_* bones (applies to avatars loaded after the change).");
			Preset = ((BaseUnityPlugin)this).Config.Bind<string>("GlutePhysics", "Preset", "visible", new ConfigDescription("How much the glutes show their motion: light, visible or strong (all toned). Read when an avatar is loaded: restart the game (or reload the avatar) after changing it.", (AcceptableValueBase)(object)new AcceptableValueList<string>(GlutePresets.Names), Array.Empty<object>()));
			Amount = ((BaseUnityPlugin)this).Config.Bind<float>("GlutePhysics", "Amount", 1f, new ConfigDescription("Strength of the shown motion: 0 = still, 1 = the preset; above 1 goes past the preset's travel limits (tested with clothing at 1 only). Live.", (AcceptableValueBase)(object)new AcceptableValueRange<float>(0f, 1.5f), Array.Empty<object>()));
			Type type = ((object)Chainloader.PluginInfos["com.rawrtastic.plugins.enhancedvalheimvrm"].Instance).GetType().Assembly.GetType("EnhancedValheimVRM.VrmInstance", throwOnError: false);
			MethodInfo methodInfo = ((type == null) ? null : AccessTools.DeclaredMethod(type, "CreateVrmGo", Type.EmptyTypes, (Type[])null));
			cloneField = ((type == null) ? null : AccessTools.DeclaredField(type, "_vrmGo"));
			if (methodInfo == null || cloneField == null)
			{
				((BaseUnityPlugin)this).Logger.LogError((object)"Inactive: EnhancedValheimVRM clone staging hook not found.");
				return;
			}
			Harmony val = new Harmony("local.valheim.nordicglutephysics");
			val.Patch((MethodBase)methodInfo, new HarmonyMethod(typeof(GlutePhysicsPlugin), "BeforeCreate", (Type[])null), (HarmonyMethod)null, (HarmonyMethod)null, (HarmonyMethod)null, (HarmonyMethod)null);
			MethodInfo methodInfo2 = AccessTools.DeclaredMethod(typeof(FastSpringBoneService), "LateUpdate", Type.EmptyTypes, (Type[])null);
			try
			{
				if (methodInfo2 == null)
				{
					throw new MissingMethodException("FastSpringBoneService.LateUpdate");
				}
				val.Patch((MethodBase)methodInfo2, (HarmonyMethod)null, new HarmonyMethod(typeof(GlutePhysicsPlugin), "AfterSprings", (Type[])null), (HarmonyMethod)null, (HarmonyMethod)null, (HarmonyMethod)null);
			}
			catch (Exception ex)
			{
				((BaseUnityPlugin)this).Logger.LogWarning((object)("Glute physics: spring service hook unavailable, avatars step in their own LateUpdate: " + ex.Message));
			}
			((BaseUnityPlugin)this).Logger.LogInfo((object)$"Glute physics prototype enabled (preset {Preset.Value}, Amount {Amount.Value:0.##}).");
		}

		private static void AfterSprings()
		{
			ServiceFrame = Time.frameCount;
			for (int num = Drivers.Count - 1; num >= 0; num--)
			{
				GluteJiggleDriver gluteJiggleDriver = Drivers[num];
				if ((Object)(object)gluteJiggleDriver == (Object)null)
				{
					Drivers.RemoveAt(num);
				}
				else
				{
					try
					{
						gluteJiggleDriver.Tick();
					}
					catch (Exception ex)
					{
						Log.LogError((object)("Glute physics step failed, driver stopped: " + ex));
						((Behaviour)gluteJiggleDriver).enabled = false;
					}
				}
			}
		}

		private static void BeforeCreate(object __instance)
		{
			if (!Enabled.Value)
			{
				return;
			}
			try
			{
				object? value = cloneField.GetValue(__instance);
				GameObject val = (GameObject)((value is GameObject) ? value : null);
				if ((Object)(object)val == (Object)null || (Object)(object)val.GetComponent<GluteJiggleDriver>() != (Object)null)
				{
					return;
				}
				Dictionary<string, Transform> dictionary = new Dictionary<string, Transform>();
				Transform[] componentsInChildren = val.GetComponentsInChildren<Transform>(true);
				foreach (Transform val2 in componentsInChildren)
				{
					if (((Object)val2).name.StartsWith("Glute_", StringComparison.Ordinal) || ((Object)val2).name == "CC_Base_Hip")
					{
						dictionary[((Object)val2).name] = val2;
					}
				}
				if (dictionary.ContainsKey("Glute_L") || dictionary.ContainsKey("Glute_R"))
				{
					GluteParams gluteParams = GlutePresets.For(Preset.Value);
					GluteJiggleDriver gluteJiggleDriver = val.AddComponent<GluteJiggleDriver>();
					if (!gluteJiggleDriver.Setup(dictionary, gluteParams, ((Object)val).name))
					{
						Object.Destroy((Object)(object)gluteJiggleDriver);
						Log.LogWarning((object)("Glute physics not attached to " + ((Object)val).name + ": glute bones not as expected."));
						return;
					}
					Log.LogInfo((object)("Glute physics attached to " + ((Object)val).name + ": preset " + Preset.Value + ", freq out/up/back " + FormattableString.Invariant($"{gluteParams.Freq[0]:0.##}/{gluteParams.Freq[1]:0.##}/{gluteParams.Freq[2]:0.##} Hz, damping {gluteParams.Zeta[1]:0.##}, inertia {gluteParams.Inertia:0.##}, steady-run share {gluteParams.Steady:0.##}.")));
				}
			}
			catch (Exception ex)
			{
				Log.LogError((object)("Glute physics setup failed: " + ex));
			}
		}
	}
	[DefaultExecutionOrder(11030)]
	public sealed class GluteJiggleDriver : MonoBehaviour
	{
		private sealed class Side
		{
			public Transform HipBone;

			public Transform ThighBone;

			public Transform Hip;

			public Transform Thigh;

			public Vector3 RestHip;

			public Vector3 RestThigh;

			public GluteSide Solver;
		}

		private readonly List<Side> sides = new List<Side>();

		private GluteParams preset;

		private int steppedFrame = -1;

		private bool loggedStep;

		private string who;

		internal bool Setup(Dictionary<string, Transform> bones, GluteParams p, string label)
		{
			//IL_00e7: Unknown result type (might be due to invalid IL or missing references)
			//IL_00ec: Unknown result type (might be due to invalid IL or missing references)
			//IL_0102: Unknown result type (might be due to invalid IL or missing references)
			//IL_0107: Unknown result type (might be due to invalid IL or missing references)
			//IL_0138: Unknown result type (might be due to invalid IL or missing references)
			//IL_0143: Unknown result type (might be due to invalid IL or missing references)
			//IL_0149: Unknown result type (might be due to invalid IL or missing references)
			//IL_0150: Unknown result type (might be due to invalid IL or missing references)
			//IL_0157: Unknown result type (might be due to invalid IL or missing references)
			//IL_015e: Unknown result type (might be due to invalid IL or missing references)
			//IL_0165: Unknown result type (might be due to invalid IL or missing references)
			//IL_01a9: Unknown result type (might be due to invalid IL or missing references)
			//IL_01ab: Unknown result type (might be due to invalid IL or missing references)
			//IL_01b1: Unknown result type (might be due to invalid IL or missing references)
			//IL_01b3: Unknown result type (might be due to invalid IL or missing references)
			who = label;
			preset = p;
			RuntimeGltfInstance component = ((Component)this).GetComponent<RuntimeGltfInstance>();
			IReadOnlyDictionary<Transform, TransformState> states = ((component != null) ? component.InitialTransformStates : null);
			if (!bones.TryGetValue("CC_Base_Hip", out var value))
			{
				return false;
			}
			string[] array = new string[2] { "L", "R" };
			foreach (string text in array)
			{
				if (bones.TryGetValue("Glute_" + text, out var value2) && bones.TryGetValue("Glute_" + text + "_Thigh", out var value3))
				{
					if ((Object)(object)value2.parent != (Object)(object)value || (Object)(object)value3.parent == (Object)null || ((Object)value3.parent).name != "CC_Base_" + text + "_ThighTwist01" || value2.childCount != 0 || value3.childCount != 0)
					{
						return false;
					}
					if (Quaternion.Angle(Local(value2, states, out var pos), Quaternion.identity) > 0.01f || Quaternion.Angle(Local(value3, states, out pos), Quaternion.identity) > 0.01f)
					{
						return false;
					}
					RestTRS(value, states, out var pos2, out var rot);
					RestTRS(value2, states, out var pos3, out var _);
					Local(value2, states, out var pos4);
					Local(value3, states, out var pos5);
					GluteSide solver = new GluteSide(D(pos4), D(pos5), M(rot), D(pos3), D(pos2), p.Clone());
					sides.Add(new Side
					{
						HipBone = value2,
						ThighBone = value3,
						Hip = value,
						Thigh = value3.parent,
						RestHip = pos4,
						RestThigh = pos5,
						Solver = solver
					});
				}
			}
			return sides.Count > 0;
		}

		private void OnEnable()
		{
			foreach (Side side in sides)
			{
				side.Solver.Reset();
			}
			if (!GlutePhysicsPlugin.Drivers.Contains(this))
			{
				GlutePhysicsPlugin.Drivers.Add(this);
			}
		}

		private void OnDisable()
		{
			//IL_0037: Unknown result type (might be due to invalid IL or missing references)
			//IL_0047: Unknown result type (might be due to invalid IL or missing references)
			//IL_0066: Unknown result type (might be due to invalid IL or missing references)
			//IL_0076: Unknown result type (might be due to invalid IL or missing references)
			GlutePhysicsPlugin.Drivers.Remove(this);
			foreach (Side side in sides)
			{
				if ((Object)(object)side.HipBone != (Object)null)
				{
					side.HipBone.localPosition = side.RestHip;
					side.HipBone.localRotation = Quaternion.identity;
				}
				if ((Object)(object)side.ThighBone != (Object)null)
				{
					side.ThighBone.localPosition = side.RestThigh;
					side.ThighBone.localRotation = Quaternion.identity;
				}
			}
		}

		private void LateUpdate()
		{
			if (GlutePhysicsPlugin.ServiceFrame >= Time.frameCount - 1)
			{
				return;
			}
			try
			{
				Tick();
			}
			catch (Exception ex)
			{
				GlutePhysicsPlugin.Log.LogError((object)("Glute physics step failed, driver stopped: " + ex));
				((Behaviour)this).enabled = false;
			}
		}

		internal void Tick()
		{
			//IL_01d0: Unknown result type (might be due to invalid IL or missing references)
			//IL_023d: Unknown result type (might be due to invalid IL or missing references)
			//IL_024e: Unknown result type (might be due to invalid IL or missing references)
			//IL_025e: Unknown result type (might be due to invalid IL or missing references)
			//IL_026e: Unknown result type (might be due to invalid IL or missing references)
			//IL_01d9: Unknown result type (might be due to invalid IL or missing references)
			//IL_01e2: Unknown result type (might be due to invalid IL or missing references)
			//IL_01e9: Unknown result type (might be due to invalid IL or missing references)
			//IL_01f1: Unknown result type (might be due to invalid IL or missing references)
			//IL_01f8: Unknown result type (might be due to invalid IL or missing references)
			//IL_0200: Unknown result type (might be due to invalid IL or missing references)
			//IL_0207: Unknown result type (might be due to invalid IL or missing references)
			//IL_020f: Unknown result type (might be due to invalid IL or missing references)
			//IL_0217: Unknown result type (might be due to invalid IL or missing references)
			//IL_021f: Unknown result type (might be due to invalid IL or missing references)
			//IL_0280: Unknown result type (might be due to invalid IL or missing references)
			//IL_028d: Unknown result type (might be due to invalid IL or missing references)
			//IL_029a: Unknown result type (might be due to invalid IL or missing references)
			//IL_02a7: Unknown result type (might be due to invalid IL or missing references)
			if (steppedFrame == Time.frameCount || !((Behaviour)this).isActiveAndEnabled)
			{
				return;
			}
			steppedFrame = Time.frameCount;
			float deltaTime = Time.deltaTime;
			if (!loggedStep)
			{
				loggedStep = true;
				GlutePhysicsPlugin.Log.LogInfo((object)($"Glute physics {who}: first step at frame {Time.frameCount}, dt {deltaTime:0.0000}, " + ((GlutePhysicsPlugin.ServiceFrame == Time.frameCount) ? "right after the spring service." : "in its own LateUpdate.")));
			}
			double amount = GlutePhysicsPlugin.Amount?.Value ?? 1f;
			Vector3 val = default(Vector3);
			Vector3 val2 = default(Vector3);
			Quaternion val3 = default(Quaternion);
			Quaternion val4 = default(Quaternion);
			foreach (Side side in sides)
			{
				if (!((Object)(object)side.HipBone == (Object)null) && !((Object)(object)side.ThighBone == (Object)null) && !((Object)(object)side.Hip == (Object)null) && !((Object)(object)side.Thigh == (Object)null))
				{
					side.Solver.Params.Amount = amount;
					GluteFrame hip = Frame(side.Hip);
					GluteFrame thigh = Frame(side.Thigh);
					side.Solver.Step(deltaTime, hip, thigh);
					side.Solver.Pose(hip, thigh, out var hipPos, out var hipRot, out var thighPos, out var thighRot);
					((Vector3)(ref val))..ctor((float)hipPos.X, (float)hipPos.Y, (float)hipPos.Z);
					((Vector3)(ref val2))..ctor((float)thighPos.X, (float)thighPos.Y, (float)thighPos.Z);
					((Quaternion)(ref val3))..ctor((float)hipRot.X, (float)hipRot.Y, (float)hipRot.Z, (float)hipRot.W);
					((Quaternion)(ref val4))..ctor((float)thighRot.X, (float)thighRot.Y, (float)thighRot.Z, (float)thighRot.W);
					if (!Finite(val) || !Finite(val2) || !Finite(new Vector3(val3.x + val4.x, val3.y + val4.y, val3.z + val4.z + val3.w + val4.w)))
					{
						side.Solver.Reset();
						side.HipBone.localPosition = side.RestHip;
						side.ThighBone.localPosition = side.RestThigh;
						side.HipBone.localRotation = Quaternion.identity;
						side.ThighBone.localRotation = Quaternion.identity;
					}
					else
					{
						side.HipBone.localPosition = val;
						side.ThighBone.localPosition = val2;
						side.HipBone.localRotation = val3;
						side.ThighBone.localRotation = val4;
					}
				}
			}
		}

		private static GluteFrame Frame(Transform t)
		{
			//IL_0001: Unknown result type (might be due to invalid IL or missing references)
			//IL_0006: Unknown result type (might be due to invalid IL or missing references)
			//IL_0008: Unknown result type (might be due to invalid IL or missing references)
			//IL_0013: Unknown result type (might be due to invalid IL or missing references)
			//IL_001d: Unknown result type (might be due to invalid IL or missing references)
			//IL_0028: Unknown result type (might be due to invalid IL or missing references)
			//IL_0034: Unknown result type (might be due to invalid IL or missing references)
			Vector3 lossyScale = t.lossyScale;
			return new GluteFrame(D(t.position), M(t.rotation), (double)(Mathf.Abs(lossyScale.x) + Mathf.Abs(lossyScale.y) + Mathf.Abs(lossyScale.z)) / 3.0);
		}

		private static Quaternion Local(Transform t, IReadOnlyDictionary<Transform, TransformState> states, out Vector3 pos)
		{
			//IL_0025: Unknown result type (might be due to invalid IL or missing references)
			//IL_002a: Unknown result type (might be due to invalid IL or missing references)
			//IL_0030: Unknown result type (might be due to invalid IL or missing references)
			//IL_0011: Unknown result type (might be due to invalid IL or missing references)
			//IL_0016: Unknown result type (might be due to invalid IL or missing references)
			//IL_001d: Unknown result type (might be due to invalid IL or missing references)
			if (states != null && states.TryGetValue(t, out var value))
			{
				pos = ((TransformState)(ref value)).LocalPosition;
				return ((TransformState)(ref value)).LocalRotation;
			}
			pos = t.localPosition;
			return t.localRotation;
		}

		private void RestTRS(Transform t, IReadOnlyDictionary<Transform, TransformState> states, out Vector3 pos, out Quaternion rot)
		{
			//IL_0001: Unknown result type (might be due to invalid IL or missing references)
			//IL_0006: Unknown result type (might be due to invalid IL or missing references)
			//IL_000d: Unknown result type (might be due to invalid IL or missing references)
			//IL_0012: Unknown result type (might be due to invalid IL or missing references)
			//IL_005b: Unknown result type (might be due to invalid IL or missing references)
			//IL_0060: Unknown result type (might be due to invalid IL or missing references)
			//IL_0063: Unknown result type (might be due to invalid IL or missing references)
			//IL_006a: Unknown result type (might be due to invalid IL or missing references)
			//IL_006f: Unknown result type (might be due to invalid IL or missing references)
			//IL_0071: Unknown result type (might be due to invalid IL or missing references)
			//IL_0076: Unknown result type (might be due to invalid IL or missing references)
			//IL_007b: Unknown result type (might be due to invalid IL or missing references)
			//IL_0084: Unknown result type (might be due to invalid IL or missing references)
			//IL_0089: Unknown result type (might be due to invalid IL or missing references)
			//IL_008a: Unknown result type (might be due to invalid IL or missing references)
			//IL_008f: Unknown result type (might be due to invalid IL or missing references)
			pos = Vector3.zero;
			rot = Quaternion.identity;
			List<Transform> list = new List<Transform>();
			Transform val = t;
			while ((Object)(object)val != (Object)null && (Object)(object)val != (Object)(object)((Component)this).transform)
			{
				list.Add(val);
				val = val.parent;
			}
			for (int num = list.Count - 1; num >= 0; num--)
			{
				Vector3 pos2;
				Quaternion val2 = Local(list[num], states, out pos2);
				pos += rot * pos2;
				rot *= val2;
			}
		}

		private static D3 D(Vector3 v)
		{
			//IL_0000: Unknown result type (might be due to invalid IL or missing references)
			//IL_0007: Unknown result type (might be due to invalid IL or missing references)
			//IL_000e: Unknown result type (might be due to invalid IL or missing references)
			return new D3(v.x, v.y, v.z);
		}

		private static M3 M(Quaternion q)
		{
			//IL_0000: Unknown result type (might be due to invalid IL or missing references)
			//IL_0001: Unknown result type (might be due to invalid IL or missing references)
			//IL_0006: Unknown result type (might be due to invalid IL or missing references)
			//IL_0010: Unknown result type (might be due to invalid IL or missing references)
			//IL_0011: Unknown result type (might be due to invalid IL or missing references)
			//IL_0016: Unknown result type (might be due to invalid IL or missing references)
			//IL_0020: Unknown result type (might be due to invalid IL or missing references)
			//IL_0021: Unknown result type (might be due to invalid IL or missing references)
			//IL_0026: Unknown result type (might be due to invalid IL or missing references)
			return new M3(D(q * Vector3.right), D(q * Vector3.up), D(q * Vector3.forward));
		}

		private static bool Finite(Vector3 v)
		{
			//IL_0000: Unknown result type (might be due to invalid IL or missing references)
			//IL_0006: Unknown result type (might be due to invalid IL or missing references)
			//IL_000d: Unknown result type (might be due to invalid IL or missing references)
			//IL_001b: Unknown result type (might be due to invalid IL or missing references)
			//IL_0021: Unknown result type (might be due to invalid IL or missing references)
			//IL_0028: Unknown result type (might be due to invalid IL or missing references)
			if (!float.IsNaN(v.x + v.y + v.z))
			{
				return !float.IsInfinity(v.x + v.y + v.z);
			}
			return false;
		}
	}
}
namespace LocalValheimFixes
{
	public struct D3
	{
		public double X;

		public double Y;

		public double Z;

		public double this[int k]
		{
			get
			{
				return k switch
				{
					1 => Y, 
					0 => X, 
					_ => Z, 
				};
			}
			set
			{
				switch (k)
				{
				case 0:
					X = value;
					break;
				case 1:
					Y = value;
					break;
				default:
					Z = value;
					break;
				}
			}
		}

		public D3(double x, double y, double z)
		{
			X = x;
			Y = y;
			Z = z;
		}

		public static D3 operator +(D3 a, D3 b)
		{
			return new D3(a.X + b.X, a.Y + b.Y, a.Z + b.Z);
		}

		public static D3 operator -(D3 a, D3 b)
		{
			return new D3(a.X - b.X, a.Y - b.Y, a.Z - b.Z);
		}

		public static D3 operator *(D3 a, double s)
		{
			return new D3(a.X * s, a.Y * s, a.Z * s);
		}
	}
	public struct M3
	{
		public D3 C0;

		public D3 C1;

		public D3 C2;

		public M3(D3 c0, D3 c1, D3 c2)
		{
			C0 = c0;
			C1 = c1;
			C2 = c2;
		}

		public D3 Col(int k)
		{
			return k switch
			{
				1 => C1, 
				0 => C0, 
				_ => C2, 
			};
		}
	}
	public struct Q4
	{
		public double X;

		public double Y;

		public double Z;

		public double W;

		public Q4(double x, double y, double z, double w)
		{
			X = x;
			Y = y;
			Z = z;
			W = w;
		}
	}
	public sealed class JiggleStage
	{
		public bool On = true;

		public double[] Freq;

		public double[] Zeta;

		public double[] LimPos;

		public double[] LimNeg;

		public JiggleStage Clone()
		{
			return new JiggleStage
			{
				On = On,
				Freq = (double[])Freq.Clone(),
				Zeta = (double[])Zeta.Clone(),
				LimPos = (double[])LimPos.Clone(),
				LimNeg = (double[])LimNeg.Clone()
			};
		}
	}
	public sealed class JiggleSupport
	{
		public bool On = true;

		public double FreqMul = 1.25;

		public double ZetaMul = 1.35;

		public double LimMul = 0.6;

		public double SideLimMul = 0.48;

		public double InertiaMul = 0.6;

		public double SwingMul = 0.5;

		public JiggleSupport Clone()
		{
			return (JiggleSupport)MemberwiseClone();
		}
	}
	public sealed class JiggleSwing
	{
		public bool On = true;

		public double Amount = 1.0;

		public double RateLo = 300.0;

		public double RateHi = 800.0;

		public double Attack = 0.06;

		public double Release = 0.45;

		public double FreqMul = 0.6;

		public double ZetaMul = 1.2;

		public double LimMul = 1.7;

		public JiggleSwing Clone()
		{
			return (JiggleSwing)MemberwiseClone();
		}
	}
	public sealed class JiggleParams
	{
		public JiggleStage Base = new JiggleStage
		{
			Freq = new double[3] { 2.5, 3.2, 5.5 },
			Zeta = new double[3] { 0.24, 0.22, 0.4 },
			LimPos = new double[3] { 0.025, 0.025, 0.012 },
			LimNeg = new double[3] { 0.025, 0.03, 0.008 }
		};

		public JiggleStage Tip = new JiggleStage
		{
			On = false,
			Freq = new double[3] { 5.0, 4.5, 6.0 },
			Zeta = new double[3] { 0.2, 0.18, 0.3 },
			LimPos = new double[3] { 0.008, 0.01, 0.005 },
			LimNeg = new double[3] { 0.008, 0.01, 0.004 }
		};

		public double Inertia = 0.45;

		public double Gravity = 0.7;

		public double Amount = 1.0;

		public double RotShare = 0.75;

		public double MaxTurnDeg = 32.0;

		public double Squash;

		public double Radius = 0.045;

		public double MarkerFrac = 0.66;

		public double SkinWeight = 0.85;

		public double FilterHz = 12.0;

		public double MaxAccel = 40.0;

		public double MaxSpeed = 1.5;

		public double Substep = 1.0 / 240.0;

		public double SoftStart = 0.8;

		public double SoftFreq = 8.0;

		public double HardAt = 1.25;

		public double Bounce;

		public JiggleSupport Support = new JiggleSupport();

		public JiggleSwing Swing = new JiggleSwing();

		public const double SupportBlendSeconds = 0.3;

		public JiggleParams Clone()
		{
			JiggleParams obj = (JiggleParams)MemberwiseClone();
			obj.Base = Base.Clone();
			obj.Tip = Tip.Clone();
			obj.Support = Support.Clone();
			obj.Swing = Swing.Clone();
			return obj;
		}

		public JiggleParams Supported()
		{
			if (!Support.On)
			{
				return this;
			}
			JiggleParams jiggleParams = Clone();
			jiggleParams.Inertia = Inertia * Support.InertiaMul;
			jiggleParams.Swing.Amount = Swing.Amount * Support.SwingMul;
			JiggleStage[] array = new JiggleStage[2] { jiggleParams.Base, jiggleParams.Tip };
			foreach (JiggleStage jiggleStage in array)
			{
				for (int j = 0; j < 3; j++)
				{
					double num = ((j == 0) ? Support.SideLimMul : Support.LimMul);
					jiggleStage.Freq[j] *= Support.FreqMul;
					jiggleStage.Zeta[j] = Math.Min(1.0, jiggleStage.Zeta[j] * Support.ZetaMul);
					jiggleStage.LimPos[j] *= num;
					jiggleStage.LimNeg[j] *= num;
				}
			}
			return jiggleParams;
		}

		public static void Blend(JiggleParams bare, JiggleParams clothed, double t, JiggleParams into)
		{
			JiggleParams jiggleParams = ((t <= 0.0) ? bare : ((t >= 1.0) ? clothed : null));
			into.Inertia = jiggleParams?.Inertia ?? (bare.Inertia + (clothed.Inertia - bare.Inertia) * t);
			into.Swing.Amount = jiggleParams?.Swing.Amount ?? (bare.Swing.Amount + (clothed.Swing.Amount - bare.Swing.Amount) * t);
			BlendStage(bare.Base, clothed.Base, t, jiggleParams?.Base, into.Base);
			BlendStage(bare.Tip, clothed.Tip, t, jiggleParams?.Tip, into.Tip);
		}

		private static void BlendStage(JiggleStage a, JiggleStage b, double t, JiggleStage src, JiggleStage into)
		{
			for (int i = 0; i < 3; i++)
			{
				into.Freq[i] = ((src != null) ? src.Freq[i] : (a.Freq[i] + (b.Freq[i] - a.Freq[i]) * t));
				into.Zeta[i] = ((src != null) ? src.Zeta[i] : (a.Zeta[i] + (b.Zeta[i] - a.Zeta[i]) * t));
				into.LimPos[i] = ((src != null) ? src.LimPos[i] : (a.LimPos[i] + (b.LimPos[i] - a.LimPos[i]) * t));
				into.LimNeg[i] = ((src != null) ? src.LimNeg[i] : (a.LimNeg[i] + (b.LimNeg[i] - a.LimNeg[i]) * t));
			}
		}
	}
	public sealed class JiggleRest
	{
		public D3 Head;

		public D3 Tail;

		public Q4 Q0;

		public M3 Basis;

		public double Hand = 1.0;
	}
	public struct JigglePose
	{
		public D3 Position;

		public D3 Scale;

		public Q4 Quaternion;
	}
	public sealed class BreastJiggle
	{
		public const double GRAVITY = 9.81;

		public readonly JiggleRest Rest;

		public JiggleParams Params;

		public D3 X;

		public D3 V;

		public D3 Xt;

		public D3 Vt;

		public D3 Accel;

		public double Swing;

		public double TwistRate;

		private readonly JiggleStage swingStage = new JiggleStage
		{
			Freq = new double[3],
			Zeta = new double[3],
			LimPos = new double[3],
			LimNeg = new double[3]
		};

		private bool hasPrev;

		private D3 prevA;

		private D3 prevV;

		private D3 prevW;

		private M3 prevM;

		private int prevN;

		private double prevDt;

		public BreastJiggle(JiggleRest rest, JiggleParams p)
		{
			Rest = rest;
			Params = p;
			Reset();
		}

		public void Reset()
		{
			X = default(D3);
			V = default(D3);
			Xt = default(D3);
			Vt = default(D3);
			hasPrev = false;
			Accel = default(D3);
			Swing = 0.0;
			TwistRate = 0.0;
		}

		public static JiggleRest MakeRest(D3 head, Q4 q0, D3 tailLocal, M3 parentRot, D3 headWorld, D3 midWorld)
		{
			D3 d = head + QRotate(q0, tailLocal);
			D3 d2 = MulMtV(parentRot, new D3(0.0, 1.0, 0.0));
			D3 d3 = d - head;
			d3 = Norm(d3 - d2 * Dot(d3, d2));
			D3 d4 = Cross(d2, d3);
			if (Dot(MulMV(parentRot, d4), headWorld - midWorld) < 0.0)
			{
				d4 *= -1.0;
			}
			double hand = ((!(Dot(Cross(d4, d2), d3) < 0.0)) ? 1 : (-1));
			return new JiggleRest
			{
				Head = head,
				Q0 = q0,
				Tail = d,
				Basis = new M3(d4, d2, d3),
				Hand = hand
			};
		}

		private D3 MarkerLocal()
		{
			return Rest.Head + (Rest.Tail - Rest.Head) * Math.Max(0.05, Params.MarkerFrac);
		}

		public void Step(double dt, D3 parentPos, M3 parentRot, double parentScale = 1.0)
		{
			JiggleParams jiggleParams = Params;
			JiggleRest rest = Rest;
			D3 d = parentPos + MulMV(parentRot, MarkerLocal()) * parentScale;
			M3 m = MulMM(parentRot, rest.Basis);
			if (!(dt > 0.0))
			{
				return;
			}
			double num = 0.0;
			if (hasPrev)
			{
				double num2 = 0.0;
				for (int i = 0; i < 3; i++)
				{
					num2 += Dot(prevM.Col(i), m.Col(i));
				}
				num = Math.Acos(Math.Max(-1.0, Math.Min(1.0, (num2 - 1.0) / 2.0)));
			}
			if (!hasPrev || dt > 0.1 || Len(d - prevA) > 1.0 || num > Math.PI / 2.0)
			{
				hasPrev = true;
				prevA = d;
				prevM = m;
				prevV = default(D3);
				prevW = default(D3);
				prevN = 0;
				prevDt = dt;
				return;
			}
			D3 d2 = default(D3);
			for (int j = 0; j < 3; j++)
			{
				d2 += Cross(prevM.Col(j), m.Col(j));
			}
			double num3 = Len(d2);
			d2 = ((num3 > 1E-12) ? (d2 * (num / (dt * num3))) : default(D3));
			D3 d3 = (d - prevA) * (1.0 / dt);
			double t = ((jiggleParams.FilterHz > 0.0) ? (1.0 - Math.Exp(Math.PI * -2.0 * jiggleParams.FilterHz * dt)) : 1.0);
			bool num4 = prevN == 0;
			D3 d4 = (num4 ? d3 : Lerp(prevV, d3, t));
			D3 d5 = (num4 ? d2 : Lerp(prevW, d2, t));
			double num5 = (prevDt + dt) / 2.0;
			D3 d6 = (num4 ? default(D3) : ((d4 - prevV) * (1.0 / num5)));
			D3 v = (num4 ? default(D3) : ((d5 - prevW) * (1.0 / num5)));
			d6 *= 1.0 / Math.Sqrt(1.0 + Dot(d6, d6) / (jiggleParams.MaxAccel * jiggleParams.MaxAccel));
			D3 accel = MulMtV(prevM, d6);
			D3 a = MulMtV(m, d5) * rest.Hand;
			D3 a2 = MulMtV(m, v) * rest.Hand;
			D3 a3 = MulMtV(prevM, new D3(0.0, -9.81, 0.0));
			D3 b = MulMtV(m, new D3(0.0, -9.81, 0.0));
			Accel = accel;
			JiggleStage jiggleStage = jiggleParams.Base;
			JiggleSwing swing = jiggleParams.Swing;
			TwistRate = a.Y;
			if (swing != null && swing.On)
			{
				double num6 = Math.Min(1.0, Math.Max(0.0, (Math.Abs(a.Y) * 180.0 / Math.PI - swing.RateLo) / (swing.RateHi - swing.RateLo)));
				double num7 = num6 * num6 * (3.0 - 2.0 * num6);
				Swing += (num7 - Swing) * (1.0 - Math.Exp((0.0 - dt) / ((num7 > Swing) ? swing.Attack : swing.Release)));
				double num8 = Swing * swing.Amount;
				if (num8 > 0.0)
				{
					JiggleStage jiggleStage2 = jiggleParams.Base;
					jiggleStage = swingStage;
					double num9 = 1.0 + (swing.FreqMul - 1.0) * num8;
					double num10 = 1.0 + (swing.ZetaMul - 1.0) * num8;
					double num11 = 1.0 + (swing.LimMul - 1.0) * num8;
					for (int k = 0; k < 3; k++)
					{
						jiggleStage.Freq[k] = jiggleStage2.Freq[k];
						jiggleStage.Zeta[k] = jiggleStage2.Zeta[k];
						jiggleStage.LimPos[k] = jiggleStage2.LimPos[k];
						jiggleStage.LimNeg[k] = jiggleStage2.LimNeg[k];
					}
					jiggleStage.Freq[0] = jiggleStage2.Freq[0] * num9;
					jiggleStage.Zeta[0] = Math.Min(1.0, jiggleStage2.Zeta[0] * num10);
					jiggleStage.LimPos[0] = jiggleStage2.LimPos[0] * num11;
					jiggleStage.LimNeg[0] = jiggleStage2.LimNeg[0] * num11;
				}
			}
			int num12 = Math.Max(1, (int)Math.Ceiling(dt / jiggleParams.Substep - 1E-09));
			double num13 = dt / (double)num12;
			for (int l = 1; l <= num12; l++)
			{
				D3 d7 = Lerp(a3, b, (double)l / (double)num12);
				D3 d8 = Cross(a, V);
				D3 d9 = Cross(a2, X);
				D3 d10 = Cross(a, Cross(a, X));
				D3 d11 = default(D3);
				D3 a4 = default(D3);
				for (int n = 0; n < 3; n++)
				{
					d11[n] = jiggleParams.Inertia * (0.0 - accel[n] - 2.0 * d8[n] - d9[n] - d10[n]) + jiggleParams.Gravity * (d7[n] - ((n == 1) ? (-9.81) : 0.0));
					double num14 = Math.PI * 2.0 * jiggleStage.Freq[n];
					a4[n] = d11[n] - num14 * num14 * X[n] - 2.0 * jiggleStage.Zeta[n] * num14 * V[n];
				}
				SoftLimit(ref a4, X, jiggleStage, parentScale, jiggleParams);
				for (int num15 = 0; num15 < 3; num15++)
				{
					V[num15] += a4[num15] * num13;
				}
				double num16 = Len(V);
				double num17 = jiggleParams.MaxSpeed * parentScale;
				if (num16 > num17)
				{
					V *= num17 / num16;
				}
				for (int num18 = 0; num18 < 3; num18++)
				{
					X[num18] += V[num18] * num13;
				}
				HardLimit(ref X, ref V, jiggleStage, parentScale, jiggleParams.HardAt, jiggleParams.Bounce);
				if (jiggleParams.Tip.On)
				{
					for (int num19 = 0; num19 < 3; num19++)
					{
						double num20 = Math.PI * 2.0 * jiggleParams.Tip.Freq[num19];
						double num21 = 0.0 - (a4[num19] - d11[num19]) - num20 * num20 * Xt[num19] - 2.0 * jiggleParams.Tip.Zeta[num19] * num20 * Vt[num19];
						Vt[num19] += num21 * num13;
						Xt[num19] += Vt[num19] * num13;
					}
					HardLimit(ref Xt, ref Vt, jiggleParams.Tip, parentScale, jiggleParams.HardAt, jiggleParams.Bounce);
				}
			}
			if (!jiggleParams.Tip.On)
			{
				Xt = default(D3);
				Vt = default(D3);
			}
			prevA = d;
			prevM = m;
			prevV = d4;
			prevW = d5;
			prevN++;
			prevDt = dt;
		}

		public D3 Displacement()
		{
			return X + Xt;
		}

		public JigglePose Pose(double parentScale = 1.0)
		{
			JiggleParams jiggleParams = Params;
			JiggleRest rest = Rest;
			M3 basis = rest.Basis;
			double num = jiggleParams.Amount / (parentScale * jiggleParams.SkinWeight);
			D3 d = MulMV(basis, X) * num;
			D3 d2 = MulMV(basis, Xt) * num;
			D3 d3 = d + d2;
			D3 d4 = MarkerLocal() - rest.Head;
			double num2 = Len(d4);
			D3 d5 = d4 * (1.0 / num2);
			D3 d6 = d * jiggleParams.RotShare + d2;
			d6 -= d5 * Dot(d6, d5);
			double num3 = num2 * Math.Tan(jiggleParams.MaxTurnDeg * Math.PI / 180.0);
			double num4 = Len(d6);
			if (num4 > num3)
			{
				d6 *= num3 / num4;
			}
			Q4 q = QFromTo(d5, Norm(d4 + d6));
			double num5 = Math.Max(-0.1, Math.Min(0.1, jiggleParams.Amount * jiggleParams.Squash * X.Y / (jiggleParams.Radius * parentScale)));
			double num6 = 1.0 - num5;
			double num7 = 1.0 / Math.Sqrt(num6);
			D3 d7 = QRotate(QConj(rest.Q0), basis.C1);
			D3 scale = ((Math.Abs(d7.X) >= Math.Abs(d7.Z)) ? new D3(num6, num7, num7) : new D3(num7, num7, num6));
			D3 d8 = QRotate(QConj(rest.Q0), d4);
			D3 d9 = QRotate(QMul(q, rest.Q0), new D3(d8.X * scale.X, d8.Y * scale.Y, d8.Z * scale.Z));
			D3 position = rest.Head + (d3 - (d9 - d4));
			return new JigglePose
			{
				Position = position,
				Quaternion = QMul(q, rest.Q0),
				Scale = scale
			};
		}

		private static double Ellipsoid(D3 x, JiggleStage lim, double s, out D3 L)
		{
			double num = 0.0;
			L = default(D3);
			for (int i = 0; i < 3; i++)
			{
				L[i] = ((x[i] >= 0.0) ? lim.LimPos[i] : lim.LimNeg[i]) * s;
				num += x[i] / L[i] * (x[i] / L[i]);
			}
			return Math.Sqrt(num);
		}

		private static void SoftLimit(ref D3 a, D3 x, JiggleStage lim, double s, JiggleParams p)
		{
			D3 L;
			double num = Ellipsoid(x, lim, s, out L);
			if (!(num <= p.SoftStart))
			{
				double num2 = (num - p.SoftStart) / (1.0 - p.SoftStart);
				double num3 = Math.PI * 2.0 * p.SoftFreq;
				double num4 = num3 * num3 * num2 * num2;
				D3 b = Norm(new D3(x.X / (L.X * L.X), x.Y / (L.Y * L.Y), x.Z / (L.Z * L.Z)));
				double num5 = Dot(x, b);
				for (int i = 0; i < 3; i++)
				{
					a[i] -= num4 * num5 * b[i];
				}
			}
		}

		private static bool HardLimit(ref D3 x, ref D3 v, JiggleStage lim, double s, double hardAt, double bounce)
		{
			D3 L;
			double num = Ellipsoid(x, lim, s, out L);
			if (num <= hardAt)
			{
				return false;
			}
			for (int i = 0; i < 3; i++)
			{
				x[i] *= hardAt / num;
			}
			D3 b = Norm(new D3(x.X / (L.X * L.X), x.Y / (L.Y * L.Y), x.Z / (L.Z * L.Z)));
			double num2 = Dot(v, b);
			if (num2 > 0.0)
			{
				for (int j = 0; j < 3; j++)
				{
					v[j] -= (1.0 + bounce) * num2 * b[j];
				}
			}
			return true;
		}

		private static double Dot(D3 a, D3 b)
		{
			return a.X * b.X + a.Y * b.Y + a.Z * b.Z;
		}

		private static D3 Cross(D3 a, D3 b)
		{
			return new D3(a.Y * b.Z - a.Z * b.Y, a.Z * b.X - a.X * b.Z, a.X * b.Y - a.Y * b.X);
		}

		private static double Len(D3 a)
		{
			return Math.Sqrt(Dot(a, a));
		}

		private static D3 Lerp(D3 a, D3 b, double t)
		{
			return new D3(a.X + (b.X - a.X) * t, a.Y + (b.Y - a.Y) * t, a.Z + (b.Z - a.Z) * t);
		}

		private static D3 Norm(D3 a)
		{
			double num = Len(a);
			if (!(num > 1E-12))
			{
				return default(D3);
			}
			return a * (1.0 / num);
		}

		private static D3 MulMV(M3 m, D3 v)
		{
			return new D3(m.C0.X * v.X + m.C1.X * v.Y + m.C2.X * v.Z, m.C0.Y * v.X + m.C1.Y * v.Y + m.C2.Y * v.Z, m.C0.Z * v.X + m.C1.Z * v.Y + m.C2.Z * v.Z);
		}

		private static D3 MulMtV(M3 m, D3 v)
		{
			return new D3(Dot(m.C0, v), Dot(m.C1, v), Dot(m.C2, v));
		}

		private static M3 MulMM(M3 a, M3 b)
		{
			return new M3(MulMV(a, b.C0), MulMV(a, b.C1), MulMV(a, b.C2));
		}

		private static Q4 QMul(Q4 q, Q4 r)
		{
			return new Q4(q.W * r.X + q.X * r.W + q.Y * r.Z - q.Z * r.Y, q.W * r.Y - q.X * r.Z + q.Y * r.W + q.Z * r.X, q.W * r.Z + q.X * r.Y - q.Y * r.X + q.Z * r.W, q.W * r.W - q.X * r.X - q.Y * r.Y - q.Z * r.Z);
		}

		private static Q4 QConj(Q4 q)
		{
			return new Q4(0.0 - q.X, 0.0 - q.Y, 0.0 - q.Z, q.W);
		}

		private static Q4 QFromTo(D3 a, D3 b)
		{
			D3 d = Cross(a, b);
			double num = 1.0 + Dot(a, b);
			if (num < 1E-08)
			{
				return new Q4(0.0, 0.0, 0.0, 1.0);
			}
			double num2 = Math.Sqrt(d.X * d.X + d.Y * d.Y + d.Z * d.Z + num * num);
			return new Q4(d.X / num2, d.Y / num2, d.Z / num2, num / num2);
		}

		public static D3 QRotate(Q4 q, D3 v)
		{
			D3 a = new D3(q.X, q.Y, q.Z);
			D3 d = Cross(a, v) * 2.0;
			return v + d * q.W + Cross(a, d);
		}
	}
}