using System;
using System.Collections.Generic;
using System.IO;
using System.Runtime.Serialization.Formatters.Binary;
using UnityEngine;

namespace PhysicsTools
{
	public class Utility
	{
		public class Calc
		{
			private float r_length;

			private float h;

			private float d;

			public Calc(float r_, float h_, float d_)
			{
				r_length = r_;
				h = h_;
				d = d_;
			}

			public float g(float s)
			{
				return (float)(2.0 * Math.Sinh(s * d / 2f) / s - Math.Sqrt(r_length * r_length - h * h));
			}

			public float dg(float s)
			{
				return (float)(2.0 * Math.Cosh(s * d / 2f) * d / (2f * s) - 2.0 * Math.Sinh(s * d / 2f) / (s * s));
			}
		}

		private const float DEG_2_RAD = 0.017453277f;

		private const float RAD_2_DEG = 57.29583f;

		public static void MatrixToTRS(Matrix4x4 mat, out Vector3 pos, out Quaternion rot, out Vector3 scale)
		{
			pos = mat.GetColumn(3);
			rot = Quaternion.LookRotation(mat.GetColumn(2), mat.GetColumn(1));
			scale = new Vector3(mat.GetColumn(0).magnitude, mat.GetColumn(1).magnitude, mat.GetColumn(2).magnitude);
		}

		private static void Swap<T>(ref T lhs, ref T rhs)
		{
			(lhs, rhs) = (rhs, lhs);
		}

		public static List<Vector3> getCatenaryPts(Vector3 vStart, Vector3 vEnd, float r_length, int N)
		{
			bool flag = false;
			int num = 100;
			float num2 = 1E-10f;
			float num3 = 1E-08f;
			float num4 = 0.5f;
			float num5 = 1E-09f;
			float num6 = 0.001f;
			if (vStart.y > vEnd.y)
			{
				Swap(ref vStart, ref vEnd);
				flag = true;
			}
			Vector3 vector = vEnd - vStart;
			float y = vector.y;
			vector.y = 0f;
			Vector3 vector2 = vector / (N - 1);
			float magnitude = vector.magnitude;
			vector.Normalize();
			float num7 = ((magnitude == 0f) ? 1f : (1f / magnitude));
			List<Vector3> list = new List<Vector3>(new Vector3[N]);
			if (Math.Abs(magnitude) < num6)
			{
				Vector3 vector3 = new Vector3((vStart.x + vEnd.x) / 2f, 0f, (vStart.z + vEnd.z) / 2f);
				if (r_length < Math.Abs(y))
				{
					for (int i = 0; i < N; i++)
					{
						list[i] = vector3 + new Vector3(0f, vStart.y + y * (i * 1f / (N - 1)), 0f);
					}
				}
				else
				{
					num7 = (r_length - Math.Abs(y)) / 2f;
					int num8 = (int)Math.Ceiling(N * num7 / r_length);
					float num9 = Math.Max(vStart.y, vEnd.y);
					float num10 = Math.Min(vStart.y, vEnd.y);
					for (int j = 0; j < N; j++)
					{
						if (j < N - num8)
						{
							float num11 = num10 - num7 - num9;
							float num12 = num11 * (j * 1f / (N - num8 - 1f));
							list[j] = vector3 + new Vector3(0f, num9 + num12, 0f);
						}
						else
						{
							float num13 = num7;
							float num14 = num13 * (j * 1f / (num8 - 1f));
							vector3 = (list[j] = vector3 + new Vector3(0f, num10 - num7 + num14, 0f));
						}
					}
				}
			}
			else if (r_length <= Math.Sqrt(magnitude * magnitude + y * y))
			{
				for (int k = 0; k < N; k++)
				{
					Vector3 value = vStart + k * vector2;
					float num15 = k * (vEnd.y - vStart.y) / (N - 1);
					value.y = vStart.y + num15;
					list[k] = value;
				}
			}
			else
			{
				Calc calc = new Calc(r_length, y, magnitude);
				for (int l = 0; l < num; l++)
				{
					float value2 = calc.g(num7);
					float value3 = calc.dg(num7);
					if (Math.Abs(value2) < num3 || Math.Abs(value3) < num2)
					{
						break;
					}
					float num16 = (0f - calc.g(num7)) / calc.dg(num7);
					float num17 = 1f;
					float num18 = num7 + num17 * num16;
					while (num18 < 0f || Math.Abs(calc.g(num18)) > Math.Abs(value2))
					{
						num17 = num4 * num17;
						if (num17 < num5)
						{
							break;
						}
						num18 = num7 + num17 * num16;
					}
					num7 = num18;
				}
				float num19 = (float)(0.5 * (Math.Log((r_length + y) / (r_length - y)) / num7 - magnitude));
				vector.y = 0f;
				vector.Normalize();
				Vector3 vector5 = vStart - num19 * vector;
				vector5.y = 0f;
				float num20 = (float)(vStart.y - Math.Cosh(num19 * num7) / num7);
				for (int m = 0; m < N; m++)
				{
					Vector3 vector6 = vStart + m * vector2;
					Vector3 vector7 = vector6;
					vector6.y = 0f;
					vector6.y = (float)(Math.Cosh((vector6 - vector5).magnitude * num7) / num7 + num20);
					list[m] = vector6;
				}
			}
			if (flag)
			{
				list.Reverse();
			}
			return list;
		}

		public static Quaternion createOrientation(Vector3 v)
		{
			if (v.x == 0f && v.y == 0f && v.z == 0f)
			{
				return Quaternion.identity;
			}
			Vector3 vector = new Vector3(0f, 1f, 0f);
			Vector3 vector2 = Vector3.Cross(v, vector);
			vector2.Normalize();
			float num = Vector3.Dot(vector, v);
			if (num > 1f)
			{
				num = 1f;
			}
			else if (num < -1f)
			{
				num = -1f;
			}
			float num2 = (float)Math.Acos(num);
			return Quaternion.AngleAxis(num2 * 180f / 3.14159f, -vector2);
		}

		public static T DeepClone<T>(T a)
		{
			using (MemoryStream memoryStream = new MemoryStream())
			{
				BinaryFormatter binaryFormatter = new BinaryFormatter();
				binaryFormatter.Serialize(memoryStream, a);
				memoryStream.Position = 0L;
				return (T)binaryFormatter.Deserialize(memoryStream);
			}
		}

		private static Dictionary<Rigidbody, List<UnityEngine.Joint>> buildMapOfJoints()
		{
			Dictionary<Rigidbody, List<UnityEngine.Joint>> dictionary = new Dictionary<Rigidbody, List<UnityEngine.Joint>>();
			UnityEngine.Joint[] array = UnityEngine.Object.FindObjectsOfType<UnityEngine.Joint>();
			UnityEngine.Joint[] array2 = array;
			foreach (UnityEngine.Joint joint in array2)
			{
				Rigidbody component = joint.gameObject.GetComponent<Rigidbody>();
				if (component != null)
				{
					if (!dictionary.ContainsKey(component))
					{
						dictionary.Add(component, new List<UnityEngine.Joint>());
					}
					dictionary[component].Add(joint);
				}
				Rigidbody connectedBody = joint.connectedBody;
				if (connectedBody != null)
				{
					if (!dictionary.ContainsKey(connectedBody))
					{
						dictionary.Add(connectedBody, new List<UnityEngine.Joint>());
					}
					dictionary[connectedBody].Add(joint);
				}
			}
			return dictionary;
		}

		private static void getConnectedBodies(ref Dictionary<Rigidbody, List<UnityEngine.Joint>> dic, Rigidbody body, ref HashSet<Rigidbody> bodies)
		{
			bodies.Add(body);
			if (!dic.ContainsKey(body))
			{
				return;
			}
			List<UnityEngine.Joint> list = dic[body];
			foreach (UnityEngine.Joint item in list)
			{
				if (item.connectedBody != null && !bodies.Contains(item.connectedBody))
				{
					getConnectedBodies(ref dic, item.connectedBody, ref bodies);
				}
				Rigidbody component = item.gameObject.GetComponent<Rigidbody>();
				if (component != null && !bodies.Contains(component))
				{
					getConnectedBodies(ref dic, component, ref bodies);
				}
			}
		}

		public static void moveConnectedBodies(Rigidbody start, Matrix4x4 newPose)
		{
			Dictionary<Rigidbody, List<UnityEngine.Joint>> dic = buildMapOfJoints();
			HashSet<Rigidbody> bodies = new HashSet<Rigidbody>();
			getConnectedBodies(ref dic, start, ref bodies);
			Matrix4x4 matrix4x = Matrix4x4.TRS(start.transform.position, start.transform.rotation, new Vector3(1f, 1f, 1f));
			foreach (Rigidbody item in bodies)
			{
				Matrix4x4 matrix4x2 = Matrix4x4.TRS(item.transform.position, item.transform.rotation, new Vector3(1f, 1f, 1f));
				Matrix4x4 inverse = matrix4x.inverse;
				matrix4x2 = inverse * matrix4x2;
				matrix4x2 = newPose * matrix4x2;
				Vector3 pos;
				Quaternion rot;
				Vector3 scale;
				MatrixToTRS(matrix4x2, out pos, out rot, out scale);
				item.transform.position = pos;
				item.transform.rotation = rot;
			}
		}
	}
}