using UnityEngine;
using System.Collections;

namespace Obi
{
    public struct ColliderRigidbody
    {
        public Matrix4x4 inverseInertiaTensor;
        public Vector4 velocity;
        public Vector4 angularVelocity;
        public Vector4 com;
        public float inverseMass;
         
        private int pad0;
        private int pad1;
        private int pad2;

        public void FromRigidbody(UnityEngine.Rigidbody rb, bool kinematicForParticles)
        {
            bool kinematic = !Application.isPlaying || rb.isKinematic || kinematicForParticles;

            //rotation = source.rotation;
            velocity = kinematicForParticles ? Vector3.zero : rb.velocity;
            angularVelocity = kinematicForParticles ? Vector3.zero : rb.angularVelocity;

            // center of mass in unity is affected by local rotation and position, but not scale. We need it expressed in world space:
            com = rb.transform.position + rb.rotation * rb.centerOfMass;

            Vector3 invTensor = new Vector3((rb.constraints & RigidbodyConstraints.FreezeRotationX) != 0 ? 0 : 1 / rb.inertiaTensor.x,
                                            (rb.constraints & RigidbodyConstraints.FreezeRotationY) != 0 ? 0 : 1 / rb.inertiaTensor.y,
                                            (rb.constraints & RigidbodyConstraints.FreezeRotationZ) != 0 ? 0 : 1 / rb.inertiaTensor.z);

            // the inertia tensor is a diagonal matrix (Vector3) because it is expressed in the space generated by the principal axes of rotation (inertiaTensorRotation).
            Vector3 inertiaTensor = kinematic ? Vector3.zero : invTensor;

            // calculate full world space inertia matrix:
            Matrix4x4 rotation = Matrix4x4.Rotate(rb.rotation * rb.inertiaTensorRotation);
            inverseInertiaTensor = rotation * Matrix4x4.Scale(inertiaTensor) * rotation.transpose;

            inverseMass = kinematic ? 0 : 1 / rb.mass;

        }

        public void FromRigidbody(UnityEngine.Rigidbody2D rb, bool kinematicForParticles)
        {

            bool kinematic = !Application.isPlaying || rb.isKinematic || kinematicForParticles;
            velocity = rb.velocity;

            // For some weird reason, in 2D angular velocity is measured in *degrees* per second, 
            // instead of radians. Seriously Unity, WTF??
            angularVelocity = new Vector4(0, 0, rb.angularVelocity * Mathf.Deg2Rad, 0);

            // center of mass in unity is affected by local rotation and poistion, but not scale. We need it expressed in world space:
            com = rb.transform.position + rb.transform.rotation * rb.centerOfMass;

            Vector3 inertiaTensor = kinematic ? Vector3.zero : new Vector3(0, 0, (rb.constraints & RigidbodyConstraints2D.FreezeRotation) != 0 ? 0 : 1 / rb.inertia);

            Matrix4x4 rotation = Matrix4x4.Rotate(Quaternion.AngleAxis(rb.rotation, Vector3.forward));
            inverseInertiaTensor = rotation * Matrix4x4.Scale(inertiaTensor) * rotation.transpose;

            inverseMass = kinematic ? 0 : 1 / rb.mass;

        }


    }
}
