
namespace BulletX.LinerMath
{
    public struct btTransform
    {
        ///Storage for the rotation
        btMatrix3x3 m_basis;
        ///Storage for the translation
        btVector3 m_origin;

        public static btTransform Identity { get { return new btTransform(btMatrix3x3.Identity); } }
        public btMatrix3x3 Basis { get { return m_basis; } set { m_basis = value; } }
        public btVector3 Origin { get { return m_origin; } set { m_origin = value; } }
        public btQuaternion Rotation
        {
            get
            {
                btQuaternion q;
                m_basis.getRotation(out q);
                return q;
            }
            set { m_basis.setRotation(ref value); }
        }
        public btTransform(btMatrix3x3 b)
        {
            m_basis = b;
            m_origin = new btVector3();
        }
        public btTransform(btMatrix3x3 b, btVector3 c)
        {
            m_basis = b;
            m_origin = c;
        }
        public btTransform(ref Microsoft.Xna.Framework.Matrix m)
        {
            /*
            m_basis.setFromOpenGLSubMatrix(m);
		    m_origin.setValue(m[12],m[13],m[14]);
             */
            /*
            matGL[0]=mat->M11;
            matGL[1]=mat->M12;
            matGL[2]=mat->M13;
            matGL[3]=mat->M14;//
            matGL[4]=mat->M21;
            matGL[5]=mat->M22;
            matGL[6]=mat->M23;
            matGL[7]=mat->M24;//
            matGL[8]=mat->M31;
            matGL[9]=mat->M32;
            matGL[10]=mat->M33;
            matGL[11]=mat->M34;//
            matGL[12]=mat->M41;
            matGL[13]=mat->M42;
            matGL[14]=mat->M43;
            matGL[15]=mat->M44;//
             */
            //移動はM41, M42, M43
            m_basis=new btMatrix3x3(ref m);
            m_origin = new btVector3(m.M41, m.M42, m.M43);
        }
        #region 演算子オーバーロード
        public static btVector3 operator *(btTransform t, btVector3 x)
        {
            return new btVector3(t.m_basis[0].dot(x) + t.m_origin.X,
                    t.m_basis[1].dot(x) + t.m_origin.Y,
                    t.m_basis[2].dot(x) + t.m_origin.Z);
        }
        public static btTransform operator*(btTransform value1, btTransform t2)
        {
	        return new btTransform(value1.m_basis * t2.m_basis, 
		        value1 * t2.m_origin);
        }
        #endregion

        //データアクセス系
        public void getXNAMatrix(out Microsoft.Xna.Framework.Matrix m)
        {
            /*
            m_basis.getOpenGLSubMatrix(m);
		    m[12] = m_origin.x();
		    m[13] = m_origin.y();
		    m[14] = m_origin.z();
		    m[15] = btScalar(1.0);
             */
            /*
            matGL[0]=mat->M11;
            matGL[1]=mat->M12;
            matGL[2]=mat->M13;
            matGL[3]=mat->M14;//
            matGL[4]=mat->M21;
            matGL[5]=mat->M22;
            matGL[6]=mat->M23;
            matGL[7]=mat->M24;//
            matGL[8]=mat->M31;
            matGL[9]=mat->M32;
            matGL[10]=mat->M33;
            matGL[11]=mat->M34;//
            matGL[12]=mat->M41;
            matGL[13]=mat->M42;
            matGL[14]=mat->M43;
            matGL[15]=mat->M44;//
             */
            m_basis.getXNASubMatrix(out m);
            m.M41 = m_origin.X;
            m.M42 = m_origin.Y;
            m.M43 = m_origin.Z;
            m.M44 = 1;
        }

        #region 演算系
        public void setIdentity()
        {
            m_basis.setIdentity();
            m_origin.setValue(0.0f, 0.0f, 0.0f);
        }
        public btTransform inverse()
        {
            btMatrix3x3 inv = m_basis.transpose();
            return new btTransform(inv, inv * -m_origin);
        }
        #endregion

        public btVector3 invXform(btVector3 inVec)
        {
            btVector3 v = inVec - m_origin;
            return (m_basis.transpose() * v);
        }

        public btTransform inverseTimes(btTransform t)
        {
            btVector3 v = t.Origin - m_origin;
            return new btTransform(m_basis.transposeTimes(t.m_basis),
                v * m_basis);
        }
    }
}
