Subversion Repositories AndroidProjects

package com.gebauz.framework.util;

public class Matrix4
{
        /* OpenGL Colum Major:
         *   m[0] m[4] m[8]  m[12]
         *   m[1] m[5] m[9]  m[13]
         *   m[2] m[6] m[10] m[14]
         *   m[3] m[7] m[11] m[15]
         */
        public float m11;
        public float m21;
        public float m31;
        public float m41;
        public float m12;
        public float m22;
        public float m32;
        public float m42;
        public float m13;
        public float m23;
        public float m33;
        public float m43;
        public float m14;
        public float m24;
        public float m34;
        public float m44;
       
        static public final int MATRIX_SIZE = 4;
       
        public Matrix4()
        {
        }
       
        public float get(int row, int col)
        {
                switch (row)
                {
                case 0:
                        switch (col)
                        {
                        case 0: return m11;
                        case 1: return m12;
                        case 2: return m13;
                        case 3: return m14;
                        }
                        break;
                case 1:
                        switch (col)
                        {
                        case 0: return m21;
                        case 1: return m22;
                        case 2: return m23;
                        case 3: return m24;
                        }
                        break;
                case 2:
                        switch (col)
                        {
                        case 0: return m31;
                        case 1: return m32;
                        case 2: return m33;
                        case 3: return m34;
                        }
                        break;
                case 3:
                        switch (col)
                        {
                        case 0: return m41;
                        case 1: return m42;
                        case 2: return m43;
                        case 3: return m44;
                        }
                        break;
                }
                // should never reach here -> TODO: assert
                return 0.0f;
        }
       
        public void set(int row, int col, float value)
        {
                switch (row)
                {
                case 0:
                        switch (col)
                        {
                        case 0:
                                m11 = value;
                                break;
                        case 1:
                                m12 = value;
                                break;
                        case 2:
                                m13 = value;
                                break;
                        case 3:
                                m14 = value;
                                break;
                        }
                        break;
                case 1:
                        switch (col)
                        {
                        case 0:
                                m21 = value;
                                break;
                        case 1:
                                m22 = value;
                                break;
                        case 2:
                                m23 = value;
                                break;
                        case 3:
                                m24 = value;
                                break;
                        }
                        break;
                case 2:
                        switch (col)
                        {
                        case 0:
                                m31 = value;
                                break;
                        case 1:
                                m32 = value;
                                break;
                        case 2:
                                m33 = value;
                                break;
                        case 3:
                                m34 = value;
                                break;
                        }
                        break;
                case 3:
                        switch (col)
                        {
                        case 0:
                                m41 = value;
                                break;
                        case 1:
                                m42 = value;
                                break;
                        case 2:
                                m43 = value;
                                break;
                        case 3:
                                m44 = value;
                                break;
                        }
                        break;
                }
        }
       
        public void zero()
        {
                m11 = 0.0f; m12 = 0.0f; m13 = 0.0f; m14 = 0.0f;        
                m21 = 0.0f; m22 = 0.0f; m23 = 0.0f; m24 = 0.0f;
                m31 = 0.0f; m32 = 0.0f; m33 = 0.0f; m34 = 0.0f;
                m41 = 0.0f; m42 = 0.0f; m43 = 0.0f; m44 = 0.0f;
        }
       
        public void identity()
        {
                zero();
                m11 = 1.0f; m22 = 1.0f; m33 = 1.0f; m44 = 1.0f;
        }
       
        public boolean equals(Matrix4 other)
        {
                return ((m11 == other.m11) && (m12 == other.m12) && (m13 == other.m13) && (m14 == other.m14) &&
                                (m21 == other.m21) && (m22 == other.m22) && (m23 == other.m23) && (m24 == other.m24) &&
                                (m31 == other.m31) && (m32 == other.m32) && (m33 == other.m33) && (m34 == other.m34) &&
                                (m41 == other.m41) && (m42 == other.m42) && (m43 == other.m43) && (m44 == other.m44));         
        }
       
        public boolean isSingular()
    {
        float determinant = getDeterminant();
        return ((float)Math.abs(determinant) < MathUtil.EPSILON);
    }
       
        public Matrix4 getTranspose()
        {
                Matrix4 result = new Matrix4();
                result.m11 = m11;
                result.m12 = m21;
                result.m13 = m31;
                result.m14 = m41;
                result.m21 = m12;
                result.m22 = m22;
                result.m23 = m32;
                result.m24 = m42;
                result.m31 = m13;
                result.m32 = m23;
                result.m33 = m33;
                result.m34 = m43;
                result.m41 = m14;
                result.m42 = m24;
                result.m43 = m34;
                result.m44 = m44;
                return result;
        }
       
        public float getDeterminant()
        {
        float result =
            m14 * m23 * m32 * m41 -
            m13 * m24 * m32 * m41 -
            m14 * m22 * m33 * m41 +
            m12 * m24 * m33 * m41 +
            m13 * m22 * m34 * m41 -
            m12 * m23 * m34 * m41 -
            m14 * m23 * m31 * m42 +
            m13 * m24 * m31 * m42 +
            m14 * m21 * m33 * m42 -
            m11 * m24 * m33 * m42 -
            m13 * m21 * m34 * m42 +
            m11 * m23 * m34 * m42 +
            m14 * m22 * m31 * m43 -
            m12 * m24 * m31 * m43 -
            m14 * m21 * m32 * m43 +
            m11 * m24 * m32 * m43 +
            m12 * m21 * m34 * m43 -
            m11 * m22 * m34 * m43 -
            m13 * m22 * m31 * m44 +
            m12 * m23 * m31 * m44 +
            m13 * m21 * m32 * m44 -
            m11 * m23 * m32 * m44 -
            m12 * m21 * m33 * m44 +
            m11 * m22 * m33 * m44;
        return result;
        }
       
        public Matrix4 getInverse()
        {
                Matrix4 result = new Matrix4();
               
                float determinant = getDeterminant();
                if (determinant == 0.0f)
                {
                        // singular matrix cannot be inverted
                        return this;
                }

        result.m11 = m23 * m34 * m42
                                 - m24 * m33 * m42
                                 + m24 * m32 * m43
                                 - m22 * m34 * m43
                                 - m23 * m32 * m44
                                 + m22 * m33 * m44;
        result.m12 = m14 * m33 * m42
                                 - m13 * m34 * m42
                                 - m14 * m32 * m43
                                 + m12 * m34 * m43
                                 + m13 * m32 * m44
                                 - m12 * m33 * m44;
        result.m13 = m13 * m24 * m42
                                 - m14 * m23 * m42
                                 + m14 * m22 * m43
                                 - m12 * m24 * m43
                                 - m13 * m22 * m44
                                 + m12 * m23 * m44;
        result.m14 = m14 * m23 * m32
                                 - m13 * m24 * m32
                                 - m14 * m22 * m33
                                 + m12 * m24 * m33
                                 + m13 * m22 * m34
                                 - m12 * m23 * m34;
        result.m21 = m24 * m33 * m41
                                 - m23 * m34 * m41
                                 - m24 * m31 * m43
                                 + m21 * m34 * m43
                                 + m23 * m31 * m44
                                 - m21 * m33 * m44;
        result.m22 = m13 * m34 * m41
                                 - m14 * m33 * m41
                                 + m14 * m31 * m43
                                 - m11 * m34 * m43
                                 - m13 * m31 * m44
                                 + m11 * m33 * m44;
        result.m23 = m14 * m23 * m41
                                 - m13 * m24 * m41
                                 - m14 * m21 * m43
                                 + m11 * m24 * m43
                                 + m13 * m21 * m44
                                 - m11 * m23 * m44;
        result.m24 = m13 * m24 * m31
                                 - m14 * m23 * m31
                                 + m14 * m21 * m33
                                 - m11 * m24 * m33
                                 - m13 * m21 * m34
                                 + m11 * m23 * m34;
        result.m31 = m22 * m34 * m41
                                 - m24 * m32 * m41
                                 + m24 * m31 * m42
                                 - m21 * m34 * m42
                                 - m22 * m31 * m44
                                 + m21 * m32 * m44;
        result.m32 = m14 * m32 * m41
                                 - m12 * m34 * m41
                                 - m14 * m31 * m42
                                 + m11 * m34 * m42
                                 + m12 * m31 * m44
                                 - m11 * m32 * m44;
        result.m33 = m12 * m24 * m41
                                 - m14 * m22 * m41
                                 + m14 * m21 * m42
                                 - m11 * m24 * m42
                                 - m12 * m21 * m44
                                 + m11 * m22 * m44;
        result.m34 = m14 * m22 * m31
                                 - m12 * m24 * m31
                                 - m14 * m21 * m32
                                 + m11 * m24 * m32
                                 + m12 * m21 * m34
                                 - m11 * m22 * m34;
        result.m41 = m23 * m32 * m41
                                 - m22 * m33 * m41
                                 - m23 * m31 * m42
                                 + m21 * m33 * m42
                                 + m22 * m31 * m43
                                 - m21 * m32 * m43;
        result.m42 = m12 * m33 * m41
                                 - m13 * m32 * m41
                                 + m13 * m31 * m42
                                 - m11 * m33 * m42
                                 - m12 * m31 * m43
                                 + m11 * m32 * m43;
        result.m43 = m13 * m22 * m41
                                 - m12 * m23 * m41
                                 - m13 * m21 * m42
                                 + m11 * m23 * m42
                                 + m12 * m21 * m43
                                 - m11 * m22 * m43;
        result.m44 = m12 * m23 * m31
                                 - m13 * m22 * m31
                                 + m13 * m21 * m32
                                 - m11 * m23 * m32
                                 - m12 * m21 * m33
                                 + m11 * m22 * m33;

                float multiply = 1.0f / determinant;
                result.m11 *= multiply;
                result.m21 *= multiply;
                result.m31 *= multiply;
                result.m41 *= multiply;
                result.m12 *= multiply;
                result.m22 *= multiply;
                result.m32 *= multiply;
                result.m42 *= multiply;
                result.m13 *= multiply;
                result.m23 *= multiply;
                result.m33 *= multiply;
                result.m43 *= multiply;
                result.m14 *= multiply;
                result.m24 *= multiply;
                result.m34 *= multiply;
                result.m44 *= multiply;

                return result;
        }

        public void copyTo(Matrix4 target)
        {
                target.m11 = m11; target.m12 = m12; target.m13 = m13; target.m14 = m14;
                target.m21 = m21; target.m22 = m22; target.m23 = m23; target.m24 = m24;
                target.m31 = m31; target.m32 = m32; target.m33 = m33; target.m34 = m34;
                target.m41 = m41; target.m42 = m42; target.m43 = m43; target.m44 = m44;
        }
       
        public void copyFrom(Matrix4 source)
        {
                m11 = source.m11; m12 = source.m12; m13 = source.m13; m14 = source.m14;
                m21 = source.m21; m22 = source.m22; m23 = source.m23; m24 = source.m24;
                m31 = source.m31; m32 = source.m32; m33 = source.m33; m34 = source.m34;
                m41 = source.m41; m42 = source.m42; m43 = source.m43; m44 = source.m44;
        }
       
        public void postMultiply(Matrix4 v)
        {
                multiply(this, this, v);
        }
       
        public void preMultiply(Matrix4 v)
        {
                multiply(this, v, this);
        }
       
        static public void multiply(Matrix4 result, Matrix4 a, Matrix4 b)
        {
        float result11 = a.m11 * b.m11 + a.m21 * b.m12 + a.m31 * b.m13 + a.m41 * b.m14;
        float result21 = a.m11 * b.m21 + a.m21 * b.m22 + a.m31 * b.m23 + a.m41 * b.m24;
        float result31 = a.m11 * b.m31 + a.m21 * b.m32 + a.m31 * b.m33 + a.m41 * b.m34;
        float result41 = a.m11 * b.m41 + a.m21 * b.m42 + a.m31 * b.m43 + a.m41 * b.m44;
        float result12 = a.m12 * b.m11 + a.m22 * b.m12 + a.m32 * b.m13 + a.m42 * b.m14;
        float result22 = a.m12 * b.m21 + a.m22 * b.m22 + a.m32 * b.m23 + a.m42 * b.m24;
        float result32 = a.m12 * b.m31 + a.m22 * b.m32 + a.m32 * b.m33 + a.m42 * b.m34;
        float result42 = a.m12 * b.m41 + a.m22 * b.m42 + a.m32 * b.m43 + a.m42 * b.m44;
        float result13 = a.m13 * b.m11 + a.m23 * b.m12 + a.m33 * b.m13 + a.m43 * b.m14;
        float result23 = a.m13 * b.m21 + a.m23 * b.m22 + a.m33 * b.m23 + a.m43 * b.m24;
                float result33 = a.m13 * b.m31 + a.m23 * b.m32 + a.m33 * b.m33 + a.m43 * b.m34;
                float result43 = a.m13 * b.m41 + a.m23 * b.m42 + a.m33 * b.m43 + a.m43 * b.m44;
                float result14 = a.m14 * b.m11 + a.m24 * b.m12 + a.m34 * b.m13 + a.m44 * b.m14;
                float result24 = a.m14 * b.m21 + a.m24 * b.m22 + a.m34 * b.m23 + a.m44 * b.m24;
                float result34 = a.m14 * b.m31 + a.m24 * b.m32 + a.m34 * b.m33 + a.m44 * b.m34;
                float result44 = a.m14 * b.m41 + a.m24 * b.m42 + a.m34 * b.m43 + a.m44 * b.m44;
                result.m11 = result11;
                result.m21 = result21;
                result.m31 = result31;
                result.m41 = result41;
                result.m12 = result12;
                result.m22 = result22;
                result.m32 = result32;
                result.m42 = result42;
                result.m13 = result13;
                result.m23 = result23;
                result.m33 = result33;
                result.m43 = result43;
                result.m14 = result14;
                result.m24 = result24;
                result.m34 = result34;
                result.m44 = result44;
        }
       
        static public Matrix4 multiply(Matrix4 a, Matrix4 b)
        {
                Matrix4 result = new Matrix4();
                multiply(result, a, b);
                return result;
        }
       
        static public Matrix4 createIdentity()
        {
                return createScale(1.0f, 1.0f, 1.0f);
        }
       
        static public Matrix4 createScale(float sx, float sy, float sz)
        {
                Matrix4 result = new Matrix4();
               
                result.m11 = sx;
                result.m22 = sy;
                result.m33 = sz;
                result.m44 = 1.0f;
               
                return result;         
        }
       
        static public Matrix4 createScale(Vector3 scale)
        {
                return createScale(scale.x, scale.y, scale.z);
        }
       
        static public Matrix4 createScale(float s)
        {
                return createScale(s, s, s);
        }
       
        static public Matrix4 createRotationX(float degrees)
        {
        float degreesrad = MathUtil.degToRad(degrees);
        float sinvalue = (float)Math.sin(degreesrad);
        float cosvalue = (float)Math.cos(degreesrad);

        Matrix4 matrix = createIdentity();
        matrix.m22 = cosvalue;
        matrix.m32 = -sinvalue;
        matrix.m23 = sinvalue;
        matrix.m33 = cosvalue;
        return matrix;
        }
       
        static public Matrix4 createRotationY(float degrees)
        {
        float degreesrad = MathUtil.degToRad(degrees);
        float sinvalue = (float)Math.sin(degreesrad);
        float cosvalue = (float)Math.cos(degreesrad);

        Matrix4 matrix = createIdentity();
        matrix.m11 = cosvalue;
        matrix.m31 = sinvalue;
        matrix.m13 = -sinvalue;
        matrix.m33 = cosvalue;
        return matrix;
        }
       
        static public Matrix4 createRotationZ(float degrees)
        {
        float degreesrad = MathUtil.degToRad(degrees);
        float sinvalue = (float)Math.sin(degreesrad);
        float cosvalue = (float)Math.cos(degreesrad);

        Matrix4 matrix = createIdentity();
        matrix.m11 = cosvalue;
        matrix.m21 = -sinvalue;
        matrix.m12 = sinvalue;
        matrix.m22 = cosvalue;
        return matrix;
        }
       
        static public Matrix4 createRotation(Vector3 axis, float degrees)
        {
        float radangle = MathUtil.degToRad(degrees);
        float radcos = (float)Math.cos(radangle);
        float radsin = (float)Math.sin(radangle);

        Matrix4 matrix = createIdentity();
        matrix.m11 = radcos + axis.x * axis.x * (1 - radcos);
        matrix.m21 = axis.z * radsin + axis.y * axis.x * (1 - radcos);
        matrix.m31 = -axis.y * radsin + axis.z * axis.x * (1 - radcos);
        matrix.m12 = -axis.z * radsin + axis.x * axis.y * (1 - radcos);
        matrix.m22 = radcos + axis.y * axis.y * (1 - radcos);
        matrix.m32 = axis.x * radsin + axis.z * axis.y * (1 - radcos);
        matrix.m13 = axis.y * radsin + axis.x * axis.z * (1 - radcos);
        matrix.m23 = -axis.x * radsin + axis.y * axis.z * (1 - radcos);
        matrix.m33 = radcos + axis.z * axis.z * (1 - radcos);
        matrix.m44 = 1.0f;
        return matrix;
        }
       
        static public Matrix4 createTranslation(float x, float y, float z)
        {
                Matrix4 result = new Matrix4();
        result.m11 = 1.0f;
        result.m21 = 0.0f;
        result.m31 = 0.0f;
        result.m41 = x;
        result.m12 = 0.0f;
        result.m22 = 1.0f;
        result.m32 = 0.0f;
        result.m42 = y;
        result.m13 = 0.0f;
        result.m23 = 0.0f;
        result.m33 = 1.0f;
        result.m43 = z;
        result.m14 = 0.0f;
        result.m24 = 0.0f;
        result.m34 = 0.0f;
        result.m44 = 1.0f;
        return result;
        }
       
        static public Matrix4 createTranslation(Vector3 translate)
        {
                return createTranslation(translate.x, translate.y, translate.z);
        }
       
        static public Matrix4 createFromVectors(Vector3 forward, Vector3 up, Vector3 right)
        {
        forward.normalize();
        up.normalize();
        right.normalize();
       
        Matrix4 result = createIdentity();
       
        result.m11 = right.x;
        result.m12 = right.y;
        result.m13 = right.z;
        result.m21 = up.x;
        result.m22 = up.y;
        result.m23 = up.z;
        result.m31 = -forward.x;
        result.m32 = -forward.y;
        result.m33 = -forward.z;
        result.m44 = 1.0f;
       
        return result;
        }
       
        public Vector3 transform(Vector3 vector)
        {
               
                return new Vector3(
                m11 * vector.x + m21 * vector.y + m31 * vector.z + m41,
                m12 * vector.x + m22 * vector.y + m32 * vector.z + m42,
                m13 * vector.x + m23 * vector.y + m33 * vector.z + m43);
        }
       
        public Vector4 transform(Vector4 vector)
        {
        return new Vector4(
                m11 * vector.x + m21 * vector.y + m31 * vector.z + m41 * vector.w,
                m12 * vector.x + m22 * vector.y + m32 * vector.z + m42 * vector.w,
                m13 * vector.x + m23 * vector.y + m33 * vector.z + m43 * vector.w,
                m14 * vector.x + m24 * vector.y + m34 * vector.z + m44 * vector.w);
        }
       
        static public void interpolate(Matrix4 result, Matrix4 a, Matrix4 b, float ratio)
        {
        result.m11 = a.m11 + (b.m11 - a.m11) * ratio;
        result.m21 = a.m21 + (b.m21 - a.m21) * ratio;
        result.m31 = a.m31 + (b.m31 - a.m31) * ratio;
        result.m41 = a.m41 + (b.m41 - a.m41) * ratio;
        result.m12 = a.m12 + (b.m12 - a.m12) * ratio;
        result.m22 = a.m22 + (b.m22 - a.m22) * ratio;
        result.m32 = a.m32 + (b.m32 - a.m32) * ratio;
        result.m42 = a.m42 + (b.m42 - a.m42) * ratio;
        result.m13 = a.m13 + (b.m13 - a.m13) * ratio;
        result.m23 = a.m23 + (b.m23 - a.m23) * ratio;
        result.m33 = a.m33 + (b.m33 - a.m33) * ratio;
        result.m43 = a.m43 + (b.m43 - a.m43) * ratio;
        result.m14 = a.m14 + (b.m14 - a.m14) * ratio;
        result.m24 = a.m24 + (b.m24 - a.m24) * ratio;
        result.m34 = a.m34 + (b.m34 - a.m34) * ratio;
        result.m44 = a.m44 + (b.m44 - a.m44) * ratio;
        }
       
        static public Matrix4 interpolate(Matrix4 a, Matrix4 b, float ratio)
        {
                Matrix4 result = createIdentity();
                interpolate(result, a, b, ratio);
                return result;
        }
       
        public Vector3 getForwardVector()
        {
                Vector3 forward = new Vector3(-m31, -m32, -m33);
        forward.normalize();
        return forward;
        }
       
        public Vector3 getUpVector()
        {
        Vector3 up = new Vector3(m21, m22, m23);
        up.normalize();
        return up;
        }
       
        public Vector3 getRightVector()
        {
        Vector3 right = new Vector3(m11, m12, m13);
        right.normalize();
        return right;
        }
       
        public float[] toGLMatrix()
        {
                float[] m = new float[16];
               
                m[0] = m11; m[1] = m21; m[2] = m31; m[3] = m41;
                m[4] = m12; m[5] = m22; m[6] = m32; m[7] = m42;
                m[8] = m13; m[9] = m23; m[10] = m33; m[11] = m43;
                m[12] = m14; m[13] = m24; m[14] = m34; m[15] = m44;
               
                return m;
        }
       
}