6 files changed, 180 insertions, 0 deletions
diff --git a/Assets/Scripts/Utils/MatrixUtility.cs b/Assets/Scripts/Utils/MatrixUtility.cs
new file mode 100644
index 00000000..f3d865f2
--- /dev/null
+++ b/Assets/Scripts/Utils/MatrixUtility.cs
@@ -0,0 +1,42 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+public static class MatrixUtility
+{
+
+	public static void SetTR(ref this Matrix4x4 matrix, Vector3 pos, Quaternion q)
+	{
+		matrix = q.ToMatrix();
+		matrix[12] = pos[0];
+		matrix[13] = pos[1];
+		matrix[14] = pos[2];
+	}
+
+	public static void SetTRS2(ref this Matrix4x4 matrix, Vector3 pos, Quaternion q, Vector3 s)
+	{
+		matrix = q.ToMatrix();
+		
+		matrix[0] *= s[0];
+		matrix[1] *= s[0];
+		matrix[2] *= s[0];
+		matrix[4] *= s[1];
+		matrix[5] *= s[1];
+		matrix[6] *= s[1];
+		matrix[8] *= s[2];
+		matrix[9] *= s[2];
+		matrix[10] *= s[2];
+		matrix[12] = pos[0];
+		matrix[13] = pos[1];
+		matrix[14] = pos[2];
+
+	}
+
+	public static Matrix4x4 RotateAndTranslate(Vector3 pos, Quaternion q)
+	{
+		Matrix4x4 m = new Matrix4x4();
+		m.SetTR(pos, q);
+		return m;
+	}
+
+}
diff --git a/Assets/Scripts/Utils/MatrixUtility.cs.meta b/Assets/Scripts/Utils/MatrixUtility.cs.meta
new file mode 100644
index 00000000..746a5d82
--- /dev/null
+++ b/Assets/Scripts/Utils/MatrixUtility.cs.meta
@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 4f4a4958b670de143b1189fc304ace4a
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
diff --git a/Assets/Scripts/Utils/QuaternionUtility.cs b/Assets/Scripts/Utils/QuaternionUtility.cs
new file mode 100644
index 00000000..d491b1fe
--- /dev/null
+++ b/Assets/Scripts/Utils/QuaternionUtility.cs
@@ -0,0 +1,51 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+public static class QuaternionUtility 
+{
+
+	public static Matrix4x4 ToMatrix(ref this Quaternion q)
+	{
+		Matrix4x4 m = new Matrix4x4();
+
+		// Precalculate coordinate products
+		float x = q.x * 2.0F;
+		float y = q.y * 2.0F;
+		float z = q.z * 2.0F;
+		float xx = q.x * x;
+		float yy = q.y * y;
+		float zz = q.z * z;
+		float xy = q.x * y;
+		float xz = q.x * z;
+		float yz = q.y * z;
+		float wx = q.w * x;
+		float wy = q.w * y;
+		float wz = q.w * z;
+
+		// Calculate 3x3 matrix from orthonormal basis
+		m[0] = 1.0f - (yy + zz);
+		m[1] = xy + wz;
+		m[2] = xz - wy;
+		m[3] = 0.0F;
+
+		m[4] = xy - wz;
+		m[5] = 1.0f - (xx + zz);
+		m[6] = yz + wx;
+		m[7] = 0.0F;
+
+		m[8] = xz + wy;
+		m[9] = yz - wx;
+		m[10] = 1.0f - (xx + yy);
+		m[11] = 0.0F;
+
+		m[12] = 0.0F;
+		m[13] = 0.0F;
+		m[14] = 0.0F;
+		m[15] = 1.0F;
+
+		return m;
+	}
+
+
+}
diff --git a/Assets/Scripts/Utils/QuaternionUtility.cs.meta b/Assets/Scripts/Utils/QuaternionUtility.cs.meta
new file mode 100644
index 00000000..49d589fc
--- /dev/null
+++ b/Assets/Scripts/Utils/QuaternionUtility.cs.meta
@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 789820cb2898f6f488d8b3bfdf94ab00
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
diff --git a/Assets/Scripts/Utils/TransformUtility.cs b/Assets/Scripts/Utils/TransformUtility.cs
new file mode 100644
index 00000000..62a28ece
--- /dev/null
+++ b/Assets/Scripts/Utils/TransformUtility.cs
@@ -0,0 +1,54 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+public static class TransformUtility
+{
+
+	public static Matrix4x4 GetLocalToWorldMatrix(Transform transform)
+	{
+		Matrix4x4 mat = Matrix4x4.identity;
+		while(transform != null)
+		{
+			Matrix4x4 m = Matrix4x4.identity;
+			m.SetTRS2(transform.localPosition, transform.localRotation, transform.localScale);
+			mat = m * mat;
+			transform = transform.parent;
+		}
+
+		return mat;
+	}
+
+	public static Matrix4x4 GetLocalToWorldMatrixNoScale(Transform transform)
+	{
+		Matrix4x4 mat = Matrix4x4.identity;
+		while (transform != null)
+		{
+			Matrix4x4 m = Matrix4x4.identity;
+			m.SetTR(transform.localPosition, transform.localRotation);
+			mat = m * mat;
+			transform = transform.parent;
+		}
+
+		return mat;
+	}
+
+	public static Matrix4x4 GetLocalToWorldMatrixRootBone(Transform transform)
+	{
+		//Matrix4x4 mat = Matrix4x4.identity;
+		//while (transform != null)
+		//{
+		//	Matrix4x4 m = Matrix4x4.identity;
+		//	m = Matrix4x4.Rotate(transform.localRotation);
+		//	mat = m * mat;
+		//	transform = transform.parent;
+		//}
+		//mat.SetColumn(3, new Vector4(trans.position.x, trans.position.y, trans.position.z, 1));
+
+		Matrix4x4 mat = Matrix4x4.Rotate(transform.rotation);
+		mat.SetColumn(3, new Vector4(transform.position.x, transform.position.y, transform.position.z, 1));
+
+		return mat;
+	}
+
+}
+\ No newline at end of file
diff --git a/Assets/Scripts/Utils/TransformUtility.cs.meta b/Assets/Scripts/Utils/TransformUtility.cs.meta
new file mode 100644
index 00000000..bda7ca18
--- /dev/null
+++ b/Assets/Scripts/Utils/TransformUtility.cs.meta
@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 00b07ec5a54ffcc44a9af249df119257
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: