matrix4x4.java

基于java的3d开发库。对坐java3d的朋友有很大的帮助。

    public Matrix4x4 cofactors()
    {
        Matrix4x4 N = new Matrix4x4();
        int row, column;
        double minor3x3[] = new double[9];
        double sign;

        for ( row = 0; row < 4; row++ ) {
            for ( column = 0; column < 4; column++ ) {
                fillMinor(minor3x3, row, column);
                if ( (row+column) % 2 == 0 ) {
                    sign = 1;
                }
                else {
                    sign = -1;
                }
                N.M[row][column] =
                    sign*determinant3x3(minor3x3);
            }
        }
        return N;
    }

    /**
     Converts current matrix into it's transpose matrix.
     */
    public void transpose()
    {
        double R[][] = new double[4][4];

        int row, column;

        for ( row = 0; row < 4; row++ ) {
            for ( column = 0; column < 4; column++ ) {
                R[row][column] = M[column][row];
            }
        }

        M = R;
    }

    /**
     Multiply this matrix by a scalar, note that this method doesn't modify 
     this matrix. 
     @param a The scalar by whom this matrix will be multiplied
     @return A new Matrix3D that contains the value of current matrix 
      multiplied by the input parameter.
     */
    public final Matrix4x4 multiply(double a)
    {
        Matrix4x4 R = new Matrix4x4();
        int row, column;

        for ( row = 0; row < 4; row++ ) {
            for ( column = 0; column < 4; column++ ) {
                R.M[row][column] = a*M[row][column];
            }
        }
        return R;
    }

    /**
     Multiply a Vector3D by this matrix. Neither this matrix nor the Vector3D 
     parameter will be modified by this method.  It returns a new Vector3D that
     represents the input vector multiplied by current matrix.
     @param E The vector to multiply by this matrix
     @return the result of the Vector3D by this matrix multiplication
     */
    public final Vector3D multiply(Vector3D E)
    {
        Vector3D R = new Vector3D();

        R.x = M[0][0] * E.x + M[0][1] * E.y + M[0][2] * E.z + M[0][3];
        R.y = M[1][0] * E.x + M[1][1] * E.y + M[1][2] * E.z + M[1][3];
        R.z = M[2][0] * E.x + M[2][1] * E.y + M[2][2] * E.z + M[2][3];

        return R;
    }

    /**
     Multiply a Vector3D by this matrix. Neither this matrix nor the Vector3D 
     parameter will be modified by this method.  It returns a new Vector3D that
     represents the input vector multiplied by current matrix.
     @param E The vector to multiply by this matrix
     @return the result of the Vector3D by this matrix multiplication
     */
    public final Vector4D multiply(Vector4D E)
    {
        Vector4D R = new Vector4D();

        R.x = M[0][0] * E.x + M[0][1] * E.y + M[0][2] * E.z + M[0][3];
        R.y = M[1][0] * E.x + M[1][1] * E.y + M[1][2] * E.z + M[1][3];
        R.z = M[2][0] * E.x + M[2][1] * E.y + M[2][2] * E.z + M[2][3];
        R.w = M[3][0] * E.x + M[3][1] * E.y + M[3][2] * E.z + M[3][3];

        return R;
    }

    /**
     This method multiplies an input matrix by this matrix, the result is a 
     new matrix and current matrix is not modified.
     @param second The matrix by whom this matrix will be multiplied
     @return The matrix result of the multiplication.
     */
    public Matrix4x4 multiply(Matrix4x4 second)
    {
        Matrix4x4 R = new Matrix4x4();
        int row_a, column_b, row_b;
        double acumulado;

        for( row_a = 0; row_a < 4; row_a++ ) {
            for( column_b = 0; column_b < 4; column_b++ ) {
                acumulado = 0;
                for( row_b = 0; row_b < 4; row_b++ ) {
                    acumulado += M[row_a][row_b]*second.M[row_b][column_b];
                }
                R.M[row_a][column_b] = acumulado;
            }
        }
        return R;
    }

    private double determinant3x3(double minor3x3[])
    {
        //return a*e*i + d*h*c + g*b*f - c*e*g - f*h*a - i*b*d;
        return minor3x3[0]*minor3x3[4]*minor3x3[8] 
             + minor3x3[3]*minor3x3[7]*minor3x3[2] 
             + minor3x3[6]*minor3x3[1]*minor3x3[5] 
             - minor3x3[2]*minor3x3[4]*minor3x3[6] 
             - minor3x3[5]*minor3x3[7]*minor3x3[0] 
             - minor3x3[8]*minor3x3[1]*minor3x3[3];
    }

    private void fillMinor(double minor3x3[], int rowPivot, int columnPivot)
    {
        int i, j;
        int index = 0;

        for ( i = 0; i < 4; i++ ) {
            for ( j = 0; j < 4; j++ ) {
                if ( i != rowPivot && j != columnPivot ) {
                    minor3x3[index] = M[i][j];
                    index++;
                }
            }
        }
    }

    /**
     This method computes the determinant of this matrix.
     @return this matrix determinant
     */
    public double determinant()
    {
        double minor3x3[] = new double[9];
        int i, j;
        double acum = 0;
        int sign;

        i = 0;
        for ( j = 0, sign = 1; j < 4; j++, sign *= -1 ) {
            fillMinor(minor3x3, i, j);
            acum += ((double)sign)*determinant3x3(minor3x3)*M[i][j];
        }
        return acum;
    }

    /**
     This method creates an String representation of this matrix, suitable
     for human interpretation. Note that the matrix values are formated,
     so precision is lost in sake of readability.
     @return The String representation of this matrix
     */
    public String toString()
    {
        String msg;

        msg = "\n------------------------------\n";
        int row, column, pos;

        for ( row = 0; row < 4; row++, pos++ ) {
            for ( pos = 0, column = 0; column < 4; column++ ) {
                msg = msg + VSDK.formatDouble(M[row][column]) + " ";
            }
            msg = msg + "\n";
        }
        msg = msg + "------------------------------\n";
        return msg;
    }

    public double[] exportToDoubleArrayRowOrder()
    {
        double array[] = new double[16];
        int i, j, k;
        for ( i = 0, k = 0; i < 4; i++ ) {
            for ( j = 0; j < 4; j++, k++ ) {
                array[k] = M[i][j];
            }
        }
        return array;
    }

    public float[] exportToFloatArrayRowOrder()
    {
        float array[] = new float[16];
        int i, j, k;
        for ( i = 0, k = 0; i < 4; i++ ) {
            for ( j = 0; j < 4; j++, k++ ) {
                array[k] = (float)M[i][j];
            }
        }
        return array;
    }

    /**
     This method creates a Quaterion equivalent to current matrix. Note that
     the resulting Quaternion will be of unit lenght if current matrix is a
     rotation one. If not, Quaternion normalization could fix a damaged 
     rotation matrix, as long resulting Quaternion is not 0, which happens
     if current matrix is nos linearly independent.
     @return The Quaterion representation of this matrix.
     */
    public Quaternion exportToQuaternion()
    {
        Quaternion quat = new Quaternion();
        double tr, s;
        double q[] = new double[4];
        int i, j, k;
        int nxt[] = new int[3];

        nxt[0] = 1;
        nxt[1] = 2;
        nxt[2] = 0;

        tr = M[0][0] + M[1][1] + M[2][2];

        // check the diagonal
        if ( tr > 0.0 ) {
            s = Math.sqrt(tr + 1.0);
            quat.magnitude = s / 2.0;
            s = 0.5 / s;
            quat.direction.x = (M[2][1] - M[1][2]) * s;
            quat.direction.y = (M[0][2] - M[2][0]) * s;
            quat.direction.z = (M[1][0] - M[0][1]) * s;
          }
          else {                
            // diagonal is negative
            i = 0;
            if (M[1][1] > M[0][0]) i = 1;
            if (M[2][2] > M[i][i]) i = 2;
            j = nxt[i];
            k = nxt[j];

            s = Math.sqrt ((M[i][i] - (M[j][j] + M[k][k])) + 1.0);

            q[i] = s * 0.5;

            if (s != 0.0) s = 0.5 / s;

            q[3] = (M[k][j] - M[j][k]) * s;
            q[j] = (M[j][i] + M[i][j]) * s;
            q[k] = (M[k][i] + M[i][k]) * s;

            quat.direction.x = q[0];
            quat.direction.y = q[1];
            quat.direction.z = q[2];
            quat.magnitude = q[3];
        }

        return quat;
    }

    /**
     This method import the information of the input Quaterion into this 
     matrix. Note that if input Quaternion is of unit lenght, the resulting
     matrix will be a rotation matrix.
     @param a The input Quaterion
     */
    public void importFromQuaternion(Quaternion a)
    {
        double sx, sy, sz, xx, yy, yz, xy, xz, zz, x2, y2, z2;

        x2 = a.direction.x + a.direction.x;
        y2 = a.direction.y + a.direction.y; 
        z2 = a.direction.z + a.direction.z;
        xx = a.direction.x * x2;
        xy = a.direction.x * y2;
        xz = a.direction.x * z2;
        yy = a.direction.y * y2;
        yz = a.direction.y * z2;
        zz = a.direction.z * z2;
        sx = a.magnitude * x2;
        sy = a.magnitude * y2;   sz = a.magnitude * z2;

        M[0][0] = 1-(yy+zz);
        M[0][1] = xy-sz;
        M[0][2] = xz+sy;
        M[0][3] = 0;

        M[1][0] = xy+sz;
        M[1][1] = 1-(xx+zz);
        M[1][2] = yz-sx;
        M[1][3] = 0;

        M[2][0] = xz-sy;
        M[2][1] = yz+sx;
        M[2][2] = 1-(xx+yy);
        M[2][3] = 0;

        M[3][0] = 0;
        M[3][1] = 0;
        M[3][2] = 0;
        M[3][3] = 1;
    }

    /**
     This method returns the yaw angle of this matrix, as interpreted if
     current matrix is a rotation one.
     @return The yaw angle of this matrix
     */
    public double obtainEulerYawAngle()
    {
        Vector3D dir = new Vector3D(1, 0, 0);
        double yaw, pitch;
        double EPSILON = 0.0004;

        pitch = obtainEulerPitchAngle();

        dir = multiply(dir);
        dir.z = 0;

        if ( Math.abs(Math.toRadians(90) - pitch) < EPSILON ) {
            dir.x = 0;  dir.y = 0;  dir.z = -1;
            dir = multiply(dir);
        }
        if ( Math.abs(Math.toRadians(-90) - pitch) < EPSILON ) {
            dir.x = 0;  dir.y = 0;  dir.z = 1;
            dir = multiply(dir);
        }
        dir.normalize();
        if ( dir.y <= 0 ) yaw = Math.asin(dir.x) - Math.toRadians(90);
        else yaw = Math.toRadians(90) - Math.asin(dir.x);

        return yaw;
    }

    /**
     This method returns the pitch angle of this matrix, as interpreted if
     current matrix is a rotation one.
     @return The pitch angle of this matrix
     */
    public double obtainEulerPitchAngle()
    {
        Vector3D dir = new Vector3D(1, 0, 0);

        dir = multiply(dir);
        dir.normalize();
        return Math.toRadians(90) - Math.acos(dir.z);
    }

    /**
     This method returns the roll angle of this matrix, as interpreted if
     current matrix is a rotation one.
     @return The roll angle of this matrix
     */
    public double obtainEulerRollAngle()
    {
        Matrix4x4 R1, R2, R3;
        double yaw, pitch, roll;

        pitch = obtainEulerPitchAngle();
        yaw = obtainEulerYawAngle();

        R3 = new Matrix4x4();
        R2 = new Matrix4x4();

        R3.axisRotation(yaw, 0, 0, 1);
        R2.axisRotation(pitch, 0, -1, 0);
        R3.invert();
        R2.invert();

        R1 = R2.multiply(R3.multiply(this));

        Quaternion q = R1.exportToQuaternion();
        q.normalize();
        R1.importFromQuaternion(q);

        if ( R1.M[2][1] >= 0 ) {  // R1.M[2][1] ::= sin(r)
            // Our angle is between 0 and 180 degrees
            roll = Math.acos(R1.M[1][1]);
          }
          else {
            // Our angle is between 180 and 360 degrees
            roll = -Math.acos(R1.M[1][1]);
        }

        return roll;
    }

}

//===========================================================================
//= EOF                                                                     =
//===========================================================================