using Microsoft.Xna.Framework;

namespace RigidBody {

	/// <summary>
	/// 質量特性
	/// </summary>
	public class MassProperty {
		float mass;
		float inverseMass;
		Matrix inertiaTensor;
		Matrix inverseInertiaTensor;

		/// <summary>
		/// 質量
		/// </summary>
		public float Mass {
			get { return mass; }
			set { mass = value; inverseMass = 1.0f / value; }
		}
		/// <summary>
		/// 質量の逆数
		/// </summary>
		public float InverseMass {
			get { return inverseMass; }
			set { mass = 1.0f / value; inverseMass = value; }
		}
		/// <summary>
		/// 慣性テンソル
		/// </summary>
		public Matrix InertiaTensor {
			get { return inertiaTensor; }
			set {
				inertiaTensor = value;
				Matrix.Invert(ref inertiaTensor, out inverseInertiaTensor);
			}
		}
		/// <summary>
		/// 逆慣性テンソル
		/// </summary>
		public Matrix InverseInertiaTensor {
			get { return inverseInertiaTensor; }
		}

		public MassProperty(float mass, Matrix inertiaTensor) {
			Mass = mass;
			InertiaTensor = inertiaTensor;
		}

		public static MassProperty CreateSphere(float radius, float density) {
			float mass;
			Matrix inertiaTensor;
			CreateInertiaTensorOfSphere(radius, density, out mass, out inertiaTensor);
			return new MassProperty(mass, inertiaTensor);
		}

		public static MassProperty CreateBox(Vector3 scale, float density) {
			float mass;
			Matrix inertiaTensor;
			CreateInertiaTensorOfBox(scale, density, out mass, out inertiaTensor);
			return new MassProperty(mass, inertiaTensor);
		}

		/// <summary>
		/// 直方体の慣性テンソル
		/// </summary>
		/// <param name="scale"></param>
		/// <param name="density"></param>
		/// <param name="mass"></param>
		/// <param name="inertiaTensor"></param>
		public static void CreateInertiaTensorOfBox(Vector3 scale, float density,
			out float mass, out Matrix inertiaTensor) {
			scale *= 2;
			mass = scale.X * scale.Y * scale.Z * density;
			var k = mass * (1.0f / 12);
			var xx = scale.X * scale.X;
			var yy = scale.Y * scale.Y;
			var zz = scale.Z * scale.Z;
			inertiaTensor = new Matrix(
				k * (yy + zz), 0, 0, 0,
				0, k * (zz + xx), 0, 0,
				0, 0, k * (xx + yy), 0,
				0, 0, 0, 1);
		}

		/// <summary>
		/// 球の慣性テンソル
		/// </summary>
		/// <param name="radius"></param>
		/// <param name="density"></param>
		/// <param name="mass"></param>
		/// <param name="inertiaTensor"></param>
		public static void CreateInertiaTensorOfSphere(float radius, float density,
				out float mass, out Matrix inertiaTensor) {
			var rr = radius * radius;
			mass = (4.0f / 3) * MathHelper.Pi * rr * radius * density;
			var k = mass * (2.0f / 5) * rr;
			inertiaTensor = new Matrix(
				k, 0, 0, 0,
				0, k, 0, 0,
				0, 0, k, 0,
				0, 0, 0, 1);
		}

		/// <summary>
		/// 四面体の慣性テンソル
		/// </summary>
		/// <param name="a"></param>
		/// <param name="b"></param>
		/// <param name="c"></param>
		/// <param name="density"></param>
		/// <param name="mass"></param>
		/// <param name="inertiaTensor"></param>
		public static void CreateInertiaTensorOfTetrahedron(Vector3 a, Vector3 b, Vector3 c, float density,
			out float mass, out Matrix inertiaTensor) {
			mass = (1.0f / 6) * Vector3.Dot(Vector3.Cross(a, b), c) * density;
			float kv = (1.0f / 20) * mass;

			Vector3 aa, bb, cc;
			Vector3.Multiply(ref a, ref a, out aa);
			Vector3.Multiply(ref b, ref b, out bb);
			Vector3.Multiply(ref c, ref c, out cc);

			float kx = a.X + b.X + c.X, kxx = aa.X + bb.X + cc.X, kxy = a.X * a.Y + b.X * b.Y + c.X * c.Y;
			float ky = a.Y + b.Y + c.Y, kyy = aa.Y + bb.Y + cc.Y, kyz = a.Y * a.Z + b.Y * b.Z + c.Y * c.Z;
			float kz = a.Z + b.Z + c.Z, kzz = aa.Z + bb.Z + cc.Z, kzx = a.Z * a.X + b.Z * b.X + c.Z * c.X;

			inertiaTensor = new Matrix(
				2 * kv * (ky * ky + kz * kz - kyy - kzz), -kv * (kx * ky + kxy), -kv * (kz * kx + kzx), 0,
				-kv * (kx * ky + kxy), 2 * kv * (kz * kz + kx * kx - kzz - kxx), -kv * (ky * kz + kyz), 0,
				-kv * (kz * kx + kzx), -kv * (ky * kz + kyz), 2 * kv * (kx * kx + ky * ky - kxx - kyy), 0,
				0, 0, 0, 1);
		}

		/// <summary>
		/// 慣性テンソルの平行移動
		/// </summary>
		/// <param name="inertiaTensor"></param>
		/// <param name="translation"></param>
		/// <param name="mass"></param>
		/// <param name="result"></param>
		public static void TranslateInertiaTensor(
			ref Matrix inertiaTensor, ref Vector3 translation, float mass, out Matrix result) {
			Vector3 k;
			Vector3.Multiply(ref translation, ref translation, out k);
			Vector3.Multiply(ref k, mass, out k);
			var xy = translation.X * translation.Y * mass;
			var yz = translation.Y * translation.Z * mass;
			var zx = translation.Z * translation.X * mass;
			result = new Matrix(
				inertiaTensor.M11 - (k.Y + k.Z), inertiaTensor.M12 + xy, inertiaTensor.M13 + zx, inertiaTensor.M14,
				inertiaTensor.M21 + xy, inertiaTensor.M22 - (k.Z + k.X), inertiaTensor.M23 + yz, inertiaTensor.M42,
				inertiaTensor.M31 + zx, inertiaTensor.M32 + yz, inertiaTensor.M33 - (k.X + k.Y), inertiaTensor.M43,
				inertiaTensor.M41, inertiaTensor.M42, inertiaTensor.M43, inertiaTensor.M44);
		}
	}

}
