matrix4x4.java

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

//===========================================================================
//=-------------------------------------------------------------------------=
//= Module history:                                                         =
//= - August 8 2005 - Oscar Chavarro: Original base version                 =
//= - August 24 2005 - David Diaz / Cesar Bustacara: Design changes to      =
//=   decouple JOGL from the Matrix data model                              =
//= - May 2 2006 - David Diaz / Oscar Chavarro: documentation added         =
//= - November 3 2006 - Oscar Chavarro: New versions of determinant and     =
//=   invert that takes into account 4x4 matrices, not just the 3x3 case    =
//=-------------------------------------------------------------------------=
//= References:                                                             =
//= [FOLE1992] Foley, vanDam, Feiner, Hughes. "Computer Graphics,           =
//=          principles and practice" - second edition, Addison Wesley,     =
//=          1992.                                                          =
//===========================================================================

package vsdk.toolkit.common;

import vsdk.toolkit.common.VSDK;

/**
This class is a data structure that represents a 4x4 matrix
 */

public class Matrix4x4 extends FundamentalEntity
{
    /// Check the general attribute description in superclass Entity.
    public static final long serialVersionUID = 20060502L;

    /** This is a 4 by 4 array of type double that represents the data of the 
        matrix */
    public double M[][];

    /**
    This constructor builds the 4x4 identity matrix:<p>
\f[
\left[
   \begin{array}{cccc}
      1 & 0 & 0 & 0 \\
      0 & 1 & 0 & 0 \\
      0 & 0 & 1 & 0 \\
      0 & 0 & 0 & 1
   \end{array}
\right]
\f]
    */
    public Matrix4x4()
    {
        M = new double[4][4];
        identity();
    }

    /**
     This constructor builds a matrix given an existing matrix, and copies
     its contents to the newly created one.
     @param B The matrix used to build this matrix data from
     */
    public Matrix4x4(Matrix4x4 B)
    {
        M = new double[4][4];
        int i,j;

        for ( i = 0; i < 4; i++ ) {
            for ( j = 0; j < 4; j++ ) {
                M[i][j] = B.M[i][j];
            }
        }
    }

    /**
    This methods changes current matrix to be the 4x4 identity matrix:<p>
\f[
\left[
   \begin{array}{cccc}
      1 & 0 & 0 & 0 \\
      0 & 1 & 0 & 0 \\
      0 & 0 & 1 & 0 \\
      0 & 0 & 0 & 1
   \end{array}
\right]
\f]
    */
    public void identity()
    {
        M[0][0]=1.0;M[0][1]=0.0;M[0][2]=0.0;M[0][3]=0.0;
        M[1][0]=0.0;M[1][1]=1.0;M[1][2]=0.0;M[1][3]=0.0;
        M[2][0]=0.0;M[2][1]=0.0;M[2][2]=1.0;M[2][3]=0.0;
        M[3][0]=0.0;M[3][1]=0.0;M[3][2]=0.0;M[3][3]=1.0;
    }

    /**
     This method calculates new values for current matrix to make it represent
     an orthogonal projection matrix.
     @param leftPlaneDistance
     @param rightPlaneDistance
     @param downPlaneDistance
     @param upPlaneDistance
     @param nearPlaneDistance
     @param farPlaneDistance

     /todo
     Complete the documentation, including the formulae for the generated 
     matrix.
     */
    public void orthogonalProjection(
        double leftPlaneDistance, double rightPlaneDistance,
        double downPlaneDistance, double upPlaneDistance,
        double nearPlaneDistance, double farPlaneDistance)
    {
        double tx, ty, tz;;

        tx = - ( (rightPlaneDistance + leftPlaneDistance) / 
                 (rightPlaneDistance - leftPlaneDistance) );
        ty = - ( (upPlaneDistance + downPlaneDistance) / 
                 (upPlaneDistance - downPlaneDistance) );
        tz = - ( (farPlaneDistance + nearPlaneDistance) / 
                 (farPlaneDistance - nearPlaneDistance) );

        M[0][0] = 2 / (rightPlaneDistance - leftPlaneDistance);
        M[0][1] = 0;
        M[0][2] = 0;
        M[0][3] = tx;

        M[1][0] = 0;
        M[1][1] = 2 / (upPlaneDistance - downPlaneDistance);
        M[1][2] = 0;
        M[1][3] = ty;

        M[2][0] = 0;
        M[2][1] = 0;
        M[2][2] = -2 / (farPlaneDistance - nearPlaneDistance);
        M[2][3] = tz;

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

    /**
    This method calculates the following new value for current matrix:
\f[
\left[
   \begin{array}{cccc}
      1 & 0 & 0 & 0 \\
      0 & 1 & 0 & 0 \\
      0 & 0 & 0 & 0 \\
      0 & 0 & -1 & 1
   \end{array}
\right]
\f]
    which correspond to the perspective projection for the VITRAL's perspective
    canonical view volume: center of projection is point <0, 0, 1>, projection
    plane is the z=0 plane, and view volume limiting planes are 45 degrees
    with respect to the z axis (fov is 90 degrees in u and v directions), that
    is, they pass by the lines x=-1, x=1, y=-1 and y=1 at the z=0 plane.
    Note that up vector for containing camera in that projection is the
    j=<0, 1, 0> vector.

    The derivation of this matrix follows the approach suggested at
    [FOLE1992].6.4. except that relations are not derived from similar
    triangles, but writting down line equations and noting that for current
    view volume, the line equations of x and y with respect to z have
    negative slopes.

    @todo document better the derivation process for this matrix, including
    drawings and algebra, step by step.
    */
    public void
    canonicalPerspectiveProjection()
    {
        identity();
        M[2][2] = 0;
        M[3][2] = -1;
    }

    /**
     This method calculates new values for current matrix to make it represent
     a perspective projection matrix, with a corresponding visualization volume
     (i.e. the frustum).
     @param leftDistance
     @param rightDistance
     @param downDistance
     @param upDistance
     @param nearPlaneDistance
     @param farPlaneDistance

     /todo
     Complete the documentation, including the formulae for the generated 
     matrix.
     */
    public void frustumProjection(
                 double leftDistance, double rightDistance,
                 double downDistance, double upDistance,
                 double nearPlaneDistance, double farPlaneDistance)
    {
        double A, B, C, D;

        A = (rightDistance + leftDistance) / (rightDistance - leftDistance);
        B = (upDistance + downDistance) / (upDistance - downDistance); 
        C = - ((farPlaneDistance + nearPlaneDistance) / (farPlaneDistance - nearPlaneDistance));
        D = - ((2 * farPlaneDistance * nearPlaneDistance) / (farPlaneDistance - nearPlaneDistance));

        M[0][0] = 2 * nearPlaneDistance / (rightDistance - leftDistance);
        M[0][1] = 0;
        M[0][2] = A;
        M[0][3] = 0;

        M[1][0] = 0;
        M[1][1] = 2 * nearPlaneDistance / (upDistance - downDistance);
        M[1][2] = B;
        M[1][3] = 0;

        M[2][0] = 0;
        M[2][1] = 0;
        M[2][2] = C;
        M[2][3] = D;

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

    /**
     This method calculates new values for current matrix to make it represent
     a translation matrix. The matrix is similar to an identity matrix, with
     recieved values in place to make the matrix a translation one.
     @param transx The translation distance in the x axis
     @param transy The translation distance in the y axis
     @param transz The translation distance in the z axis
     */
    public void
    translation(double transx, double transy, double transz)
    {
        M[0][0]=1.0; M[0][1]=0.0; M[0][2]=0.0; M[0][3]=transx;
        M[1][0]=0.0; M[1][1]=1.0; M[1][2]=0.0; M[1][3]=transy;
        M[2][0]=0.0; M[2][1]=0.0; M[2][2]=1.0; M[2][3]=transz;
        M[3][0]=0.0; M[3][1]=0.0; M[3][2]=0.0; M[3][3]=1.0;
    }

    /**
     This method calculates new values for current matrix to make it represent
     a scale matrix. The matrix is similar to an identity matrix, with
     recieved values in place to make the matrix a scale one.
     @param sx The scale factor in the x axis
     @param sy The scale factor in the y axis
     @param sz The scale factor in the z axis
     */
    public void
    scale(double sx, double sy, double sz)
    {
        M[0][0]=sx;  M[0][1]=0.0; M[0][2]=0.0; M[0][3]=0.0;
        M[1][0]=0.0; M[1][1]=sy;  M[1][2]=0.0; M[1][3]=0.0;
        M[2][0]=0.0; M[2][1]=0.0; M[2][2]=sz;  M[2][3]=0.0;
        M[3][0]=0.0; M[3][1]=0.0; M[3][2]=0.0; M[3][3]=1.0;
    }

    /**
     This method calculates new values for current matrix to make it represent
     a scale matrix. The matrix is similar to an identity matrix, with
     recieved values in place to make the matrix a scale one.
     @param s The scale factor
     */
    public void
    scale(Vector3D s)
    {
        M[0][0]=s.x;  M[0][1]=0.0; M[0][2]=0.0; M[0][3]=0.0;
        M[1][0]=0.0; M[1][1]=s.y;  M[1][2]=0.0; M[1][3]=0.0;
        M[2][0]=0.0; M[2][1]=0.0; M[2][2]=s.z;  M[2][3]=0.0;
        M[3][0]=0.0; M[3][1]=0.0; M[3][2]=0.0; M[3][3]=1.0;
    }

    /**
     This method calculates new values for current matrix to make it represent
     a translation matrix. The position of translation for the matrix is the
     one indicated by the recieved vector.
     @param T A Vector3D that contains a position to calculate the 
     translation matrix values which transform the origin to this vector.
     */
    public void
    translation(Vector3D T)
    {
        M[0][0]=1.0; M[0][1]=0.0; M[0][2]=0.0; M[0][3]=T.x;
        M[1][0]=0.0; M[1][1]=1.0; M[1][2]=0.0; M[1][3]=T.y;
        M[2][0]=0.0; M[2][1]=0.0; M[2][2]=1.0; M[2][3]=T.z;
        M[3][0]=0.0; M[3][1]=0.0; M[3][2]=0.0; M[3][3]=1.0;
    }

    /**
     This method constructs in `this` object a matrix that rotates a point in 
     the x, y and z axis, yaw, pitch and roll radians respectivelly.
     @param yaw The radians to rotate in the x axis
     @param pitch The radians to rotate in the y axis
     @param roll The radians to rotate in the z axis
     */
    public void
    eulerAnglesRotation(double yaw, double pitch, double roll)
    {
        Matrix4x4 R1, R2, R3;

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

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

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

    /**
     This method calculates new values for current matrix into a rotation 
     matrix that rotates a point in space arround a defined axis by some angle 
     in radians. The axis is specified as a vector.
     @param angle The angle in radians for the matrix
     @param axis The axis to which the point in space will rotate arround
     */
    public void
    axisRotation(double angle, Vector3D axis)
    {
        axisRotation(angle, axis.x, axis.y, axis.z);
    }

    /**
     This method calculates new values for current matrix into a rotation 
     matrix that rotates a point in space arround a defined axis by some angle
     in radians. The axis is specified as separate vector coordinates.
     @param angle The angle in radians for the matrix
     @param x X value of the axis to which the point in space will rotate arround
     @param y Y value of the axis to which the point in space will rotate arround
     @param z Z value of the axis to which the point in space will rotate arround
     */
    public void
    axisRotation(double angle, double x, double y, double z)
    {
        double mag, s, c;
        double xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;

        s = Math.sin( angle );
        c = Math.cos( angle );

        mag = Math.sqrt(x*x + y*y + z*z);

        // OJO: Propenso a error... deberia ser si es menor a EPSILON
        if ( mag == 0.0 ) {
            identity();
            return;
        }

        x /= mag;
        y /= mag;
        z /= mag;

        //-----------------------------------------------------------------
        xx = x * x;
        yy = y * y;
        zz = z * z;
        xy = x * y;
        yz = y * z;
        zx = z * x;
        xs = x * s;
        ys = y * s;
        zs = z * s;
        one_c = 1 - c;

        M[0][0] = (one_c * xx) + c;
        M[0][1] = (one_c * xy) - zs;
        M[0][2] = (one_c * zx) + ys;
        M[0][3] = 0;

        M[1][0] = (one_c * xy) + zs;
        M[1][1] = (one_c * yy) + c;
        M[1][2] = (one_c * yz) - xs;
        M[1][3] = 0;

        M[2][0] = (one_c * zx) - ys;
        M[2][1] = (one_c * yz) + xs;
        M[2][2] = (one_c * zz) + c;
        M[2][3] = 0;

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

    public Matrix4x4 inverse()
    {
        Matrix4x4 i = new Matrix4x4(this);
        i.invert();
        return i;
    }

    /**
     Converts current matrix into it's invert matrix.
     */
    public void invert()
    {
        double a = 1/determinant();
        Matrix4x4 N = cofactors(), N2;
        N.transpose();
        N2 = N.multiply(a);
        this.M = N2.M;
    }