271 lines
7.4 KiB
C#
271 lines
7.4 KiB
C#
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;
|
|
}
|
|
}
|
|
}
|
|
}
|