matrix4f.java

java 3d game jme 工程开发源代码

    public Vector3f mult(Vector3f vec, Vector3f store) {
        if (store == null) store = new Vector3f();
        
        float vx = vec.x, vy = vec.y, vz = vec.z;
        store.x = m00 * vx + m01 * vy + m02 * vz + m03;
        store.y = m10 * vx + m11 * vy + m12 * vz + m13;
        store.z = m20 * vx + m21 * vy + m22 * vz + m23;

        return store;
    }

    /**
     * <code>mult</code> multiplies a vector about a rotation matrix. The
     * resulting vector is returned.
     * 
     * @param vec
     *            vec to multiply against.
     * @param store
     *            a vector to store the result in.  created if null is passed.
     * @return the rotated vector.
     */
    public Vector3f multAcross(Vector3f vec, Vector3f store) {
        if (null == vec) {
            logger.info("Source vector is null, null result returned.");
            return null;
        }
        if (store == null) store = new Vector3f();
        
        float vx = vec.x, vy = vec.y, vz = vec.z;
        store.x = m00 * vx + m10 * vy + m20 * vz + m30 * 1;
        store.y = m01 * vx + m11 * vy + m21 * vz + m31 * 1;
        store.z = m02 * vx + m12 * vy + m22 * vz + m32 * 1;

        return store;
    }

    /**
     * <code>mult</code> multiplies a quaternion about a matrix. The
     * resulting vector is returned.
     *
     * @param vec
     *            vec to multiply against.
     * @param store
     *            a quaternion to store the result in.  created if null is passed.
     * @return store = this * vec
     */
    public Quaternion mult(Quaternion vec, Quaternion store) {

        if (null == vec) {
            logger.warning("Source vector is null, null result returned.");
            return null;
        }
        if (store == null) store = new Quaternion();

        float x = m00 * vec.x + m10 * vec.y + m20 * vec.z + m30 * vec.w;
        float y = m01 * vec.x + m11 * vec.y + m21 * vec.z + m31 * vec.w;
        float z = m02 * vec.x + m12 * vec.y + m22 * vec.z + m32 * vec.w;
        float w = m03 * vec.x + m13 * vec.y + m23 * vec.z + m33 * vec.w;
        store.x = x;
        store.y = y;
        store.z = z;
        store.w = w;

        return store;
    }
    
    /**
     * <code>mult</code> multiplies an array of 4 floats against this rotation 
     * matrix. The results are stored directly in the array. (vec4f x mat4f)
     * 
     * @param vec4f
     *            float array (size 4) to multiply against the matrix.
     * @return the vec4f for chaining.
     */
    public float[] mult(float[] vec4f) {
        if (null == vec4f || vec4f.length != 4) {
            logger.warning("invalid array given, must be nonnull and length 4");
            return null;
        }

        float x = vec4f[0], y = vec4f[1], z = vec4f[2], w = vec4f[3];
        
        vec4f[0] = m00 * x + m01 * y + m02 * z + m03 * w;
        vec4f[1] = m10 * x + m11 * y + m12 * z + m13 * w;
        vec4f[2] = m20 * x + m21 * y + m22 * z + m23 * w;
        vec4f[3] = m30 * x + m31 * y + m32 * z + m33 * w;

        return vec4f;
    }

    /**
     * <code>mult</code> multiplies an array of 4 floats against this rotation 
     * matrix. The results are stored directly in the array. (vec4f x mat4f)
     * 
     * @param vec4f
     *            float array (size 4) to multiply against the matrix.
     * @return the vec4f for chaining.
     */
    public float[] multAcross(float[] vec4f) {
        if (null == vec4f || vec4f.length != 4) {
            logger.warning("invalid array given, must be nonnull and length 4");
            return null;
        }

        float x = vec4f[0], y = vec4f[1], z = vec4f[2], w = vec4f[3];
        
        vec4f[0] = m00 * x + m10 * y + m20 * z + m30 * w;
        vec4f[1] = m01 * x + m11 * y + m21 * z + m31 * w;
        vec4f[2] = m02 * x + m12 * y + m22 * z + m32 * w;
        vec4f[3] = m03 * x + m13 * y + m23 * z + m33 * w;

        return vec4f;
    }

    /**
     * Inverts this matrix as a new Matrix4f.
     * 
     * @return The new inverse matrix
     */
    public Matrix4f invert() {
        return invert(null);
    }

    /**
     * Inverts this matrix and stores it in the given store.
     * 
     * @return The store
     */
    public Matrix4f invert(Matrix4f store) {
        if (store == null) store = new Matrix4f();

        float fA0 = m00*m11 - m01*m10;
        float fA1 = m00*m12 - m02*m10;
        float fA2 = m00*m13 - m03*m10;
        float fA3 = m01*m12 - m02*m11;
        float fA4 = m01*m13 - m03*m11;
        float fA5 = m02*m13 - m03*m12;
        float fB0 = m20*m31 - m21*m30;
        float fB1 = m20*m32 - m22*m30;
        float fB2 = m20*m33 - m23*m30;
        float fB3 = m21*m32 - m22*m31;
        float fB4 = m21*m33 - m23*m31;
        float fB5 = m22*m33 - m23*m32;
        float fDet = fA0*fB5-fA1*fB4+fA2*fB3+fA3*fB2-fA4*fB1+fA5*fB0;

        if ( FastMath.abs(fDet) <= FastMath.FLT_EPSILON )
            throw new ArithmeticException("This matrix cannot be inverted");

        store.m00 = + m11*fB5 - m12*fB4 + m13*fB3;
        store.m10 = - m10*fB5 + m12*fB2 - m13*fB1;
        store.m20 = + m10*fB4 - m11*fB2 + m13*fB0;
        store.m30 = - m10*fB3 + m11*fB1 - m12*fB0;
        store.m01 = - m01*fB5 + m02*fB4 - m03*fB3;
        store.m11 = + m00*fB5 - m02*fB2 + m03*fB1;
        store.m21 = - m00*fB4 + m01*fB2 - m03*fB0;
        store.m31 = + m00*fB3 - m01*fB1 + m02*fB0;
        store.m02 = + m31*fA5 - m32*fA4 + m33*fA3;
        store.m12 = - m30*fA5 + m32*fA2 - m33*fA1;
        store.m22 = + m30*fA4 - m31*fA2 + m33*fA0;
        store.m32 = - m30*fA3 + m31*fA1 - m32*fA0;
        store.m03 = - m21*fA5 + m22*fA4 - m23*fA3;
        store.m13 = + m20*fA5 - m22*fA2 + m23*fA1;
        store.m23 = - m20*fA4 + m21*fA2 - m23*fA0;
        store.m33 = + m20*fA3 - m21*fA1 + m22*fA0;

        float fInvDet = 1.0f/fDet;
        store.multLocal(fInvDet);

        return store;
    }

    /**
     * Inverts this matrix locally.
     * 
     * @return this
     */
    public Matrix4f invertLocal() {

        float fA0 = m00*m11 - m01*m10;
        float fA1 = m00*m12 - m02*m10;
        float fA2 = m00*m13 - m03*m10;
        float fA3 = m01*m12 - m02*m11;
        float fA4 = m01*m13 - m03*m11;
        float fA5 = m02*m13 - m03*m12;
        float fB0 = m20*m31 - m21*m30;
        float fB1 = m20*m32 - m22*m30;
        float fB2 = m20*m33 - m23*m30;
        float fB3 = m21*m32 - m22*m31;
        float fB4 = m21*m33 - m23*m31;
        float fB5 = m22*m33 - m23*m32;
        float fDet = fA0*fB5-fA1*fB4+fA2*fB3+fA3*fB2-fA4*fB1+fA5*fB0;

        if ( FastMath.abs(fDet) <= FastMath.FLT_EPSILON )
            return zero();

        float f00 = + m11*fB5 - m12*fB4 + m13*fB3;
        float f10 = - m10*fB5 + m12*fB2 - m13*fB1;
        float f20 = + m10*fB4 - m11*fB2 + m13*fB0;
        float f30 = - m10*fB3 + m11*fB1 - m12*fB0;
        float f01 = - m01*fB5 + m02*fB4 - m03*fB3;
        float f11 = + m00*fB5 - m02*fB2 + m03*fB1;
        float f21 = - m00*fB4 + m01*fB2 - m03*fB0;
        float f31 = + m00*fB3 - m01*fB1 + m02*fB0;
        float f02 = + m31*fA5 - m32*fA4 + m33*fA3;
        float f12 = - m30*fA5 + m32*fA2 - m33*fA1;
        float f22 = + m30*fA4 - m31*fA2 + m33*fA0;
        float f32 = - m30*fA3 + m31*fA1 - m32*fA0;
        float f03 = - m21*fA5 + m22*fA4 - m23*fA3;
        float f13 = + m20*fA5 - m22*fA2 + m23*fA1;
        float f23 = - m20*fA4 + m21*fA2 - m23*fA0;
        float f33 = + m20*fA3 - m21*fA1 + m22*fA0;
        
        m00 = f00;
        m01 = f01;
        m02 = f02;
        m03 = f03;
        m10 = f10;
        m11 = f11;
        m12 = f12;
        m13 = f13;
        m20 = f20;
        m21 = f21;
        m22 = f22;
        m23 = f23;
        m30 = f30;
        m31 = f31;
        m32 = f32;
        m33 = f33;

        float fInvDet = 1.0f/fDet;
        multLocal(fInvDet);

        return this;
    }
    
    /**
     * Returns a new matrix representing the adjoint of this matrix.
     * 
     * @return The adjoint matrix
     */
    public Matrix4f adjoint() {
        return adjoint(null);
    }
     
    
    /**
     * Places the adjoint of this matrix in store (creates store if null.)
     * 
     * @param store
     *            The matrix to store the result in.  If null, a new matrix is created.
     * @return store
     */
    public Matrix4f adjoint(Matrix4f store) {
        if (store == null) store = new Matrix4f();

        float fA0 = m00*m11 - m01*m10;
        float fA1 = m00*m12 - m02*m10;
        float fA2 = m00*m13 - m03*m10;
        float fA3 = m01*m12 - m02*m11;
        float fA4 = m01*m13 - m03*m11;
        float fA5 = m02*m13 - m03*m12;
        float fB0 = m20*m31 - m21*m30;
        float fB1 = m20*m32 - m22*m30;
        float fB2 = m20*m33 - m23*m30;
        float fB3 = m21*m32 - m22*m31;
        float fB4 = m21*m33 - m23*m31;
        float fB5 = m22*m33 - m23*m32;

        store.m00 = + m11*fB5 - m12*fB4 + m13*fB3;
        store.m10 = - m10*fB5 + m12*fB2 - m13*fB1;
        store.m20 = + m10*fB4 - m11*fB2 + m13*fB0;
        store.m30 = - m10*fB3 + m11*fB1 - m12*fB0;
        store.m01 = - m01*fB5 + m02*fB4 - m03*fB3;
        store.m11 = + m00*fB5 - m02*fB2 + m03*fB1;
        store.m21 = - m00*fB4 + m01*fB2 - m03*fB0;
        store.m31 = + m00*fB3 - m01*fB1 + m02*fB0;
        store.m02 = + m31*fA5 - m32*fA4 + m33*fA3;
        store.m12 = - m30*fA5 + m32*fA2 - m33*fA1;
        store.m22 = + m30*fA4 - m31*fA2 + m33*fA0;
        store.m32 = - m30*fA3 + m31*fA1 - m32*fA0;
        store.m03 = - m21*fA5 + m22*fA4 - m23*fA3;
        store.m13 = + m20*fA5 - m22*fA2 + m23*fA1;
        store.m23 = - m20*fA4 + m21*fA2 - m23*fA0;
        store.m33 = + m20*fA3 - m21*fA1 + m22*fA0;

        return store;
    }

    /**
     * <code>determinant</code> generates the determinate of this matrix.
     * 
     * @return the determinate
     */
    public float determinant() {
        float fA0 = m00*m11 - m01*m10;
        float fA1 = m00*m12 - m02*m10;
        float fA2 = m00*m13 - m03*m10;
        float fA3 = m01*m12 - m02*m11;
        float fA4 = m01*m13 - m03*m11;
        float fA5 = m02*m13 - m03*m12;
        float fB0 = m20*m31 - m21*m30;
        float fB1 = m20*m32 - m22*m30;
        float fB2 = m20*m33 - m23*m30;
        float fB3 = m21*m32 - m22*m31;
        float fB4 = m21*m33 - m23*m31;
        float fB5 = m22*m33 - m23*m32;
        float fDet = fA0*fB5-fA1*fB4+fA2*fB3+fA3*fB2-fA4*fB1+fA5*fB0;
        return fDet;
    }

    /**
     * Sets all of the values in this matrix to zero.
     * 
     * @return this matrix
     */
    public Matrix4f zero() {
        m00 = m01 = m02 = m03 = 0.0f;
        m10 = m11 = m12 = m13 = 0.0f;
        m20 = m21 = m22 = m23 = 0.0f;
        m30 = m31 = m32 = m33 = 0.0f;
        return this;
    }
    
    public Matrix4f add(Matrix4f mat) {
    	Matrix4f result = new Matrix4f();
    	result.m00 = this.m00 + mat.m00;
    	result.m01 = this.m01 + mat.m01;
    	result.m02 = this.m02 + mat.m02;
    	result.m03 = this.m03 + mat.m03;
    	result.m10 = this.m10 + mat.m10;
    	result.m11 = this.m11 + mat.m11;
    	result.m12 = this.m12 + mat.m12;
    	result.m13 = this.m13 + mat.m13;
    	result.m20 = this.m20 + mat.m20;
    	result.m21 = this.m21 + mat.m21;
    	result.m22 = this.m22 + mat.m22;
    	result.m23 = this.m23 + mat.m23;
    	result.m30 = this.m30 + mat.m30;
    	result.m31 = this.m31 + mat.m31;
    	result.m32 = this.m32 + mat.m32;
    	result.m33 = this.m33 + mat.m33;
    	return result;
    }

    /**
     * <code>add</code> adds the values of a parameter matrix to this matrix.
     * 
     * @param mat
     *            the matrix to add to this.
     */
    public void addLocal(Matrix4f mat) {
        m00 += mat.m00;
        m01 += mat.m01;
        m02 += mat.m02;
        m03 += mat.m03;
        m10 += mat.m10;
        m11 += mat.m11;
        m12 += mat.m12;
        m13 += mat.m13;
        m20 += mat.m20;
        m21 += mat.m21;
        m22 += mat.m22;
        m23 += mat.m23;
        m30 += mat.m30;
        m31 += mat.m31;
        m32 += mat.m32;
        m33 += mat.m33;
    }
    
    public Vector3f toTranslationVector() {
        return new Vector3f(m03, m13, m23);
    }
    
    public void toTranslationVector(Vector3f vector) {
        vector.set(m03, m13, m23);
    }
    
    public Quaternion toRotationQuat() {
        Quaternion quat = new Quaternion();
        quat.fromRotationMatrix(toRotationMatrix());
        return quat;
    }
    
    public void toRotationQuat(Quaternion q) {
        q.fromRotationMatrix(toRotationMatrix());
    }
    
    public Matrix3f toRotationMatrix() {
        return new Matrix3f(m00, m01, m02, m10, m11, m12, m20, m21, m22);
        
    }
    
    public void toRotationMatrix(Matrix3f mat) {
        mat.m00 = m00;
        mat.m01 = m01;
        mat.m02 = m02;
        mat.m10 = m10;
        mat.m11 = m11;
        mat.m12 = m12;
        mat.m20 = m20;
        mat.m21 = m21;
        mat.m22 = m22;
        
    }

    /**
     * <code>setTranslation</code> will set the matrix's translation values.
     * 
     * @param translation
     *            the new values for the translation.
     * @throws JmeException
     *             if translation is not size 3.
     */
    public void setTranslation(float[] translation) {
        if (translation.length != 3) { throw new JmeException(
                "Translation size must be 3."); }
        m03 = translation[0];
        m13 = translation[1];
        m23 = translation[2];
    }

    /**
     * <code>setTranslation</code> will set the matrix's translation values.
     * 
     * @param x
     *            value of the translation on the x axis
     * @param y
     *            value of the translation on the y axis
     * @param z
     *            value of the translation on the z axis
     */
    public void setTranslation(float x, float y, float z) {
        m03 = x;
        m13 = y;
        m23 = z;
    }

    /**
     * <code>setTranslation</code> will set the matrix's translation values.
     *
     * @param translation
     *            the new values for the translation.
     */
    public void setTranslation(Vector3f translation) {
        m03 = translation.x;
        m13 = translation.y;
        m23 = translation.z;