// Aqsis
// Copyright (c) 1997 - 2001, Paul C. Gregory
//
// Contact: pgregory@aqsis.com
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public
// License as published by the Free Software Foundation; either
// version 2 of the License, or (at your option) any later version.
//
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

/**
 * Copyright (C) 2006-2007  NTT DATA CORPORATION
 * 
 * Version: 1.0.0 2007/04/01
 *  
 */
package net.cellcomputing.himawari.library.types;

import static java.lang.Math.acos;
import static java.lang.Math.cos;
import static java.lang.Math.sin;
import static java.lang.Math.sqrt;
import static java.lang.Math.tan;

import java.util.logging.Logger;


import net.cellcomputing.himawari.util.HimawariLogger;


/**
 * 4x4の行列。<br>
 * 
 * 
 * @author NTT DATA Corporation
 */
public strictfp class CqMatrix {

	private static final long serialVersionUID = -1422529258858713988L;
	
	private static final float PI = (float)Math.PI;
    public float	m_aaElement[ ][ ] = new float[4][4];		///< The 4x4 array of float values.

	
	/**
	 * 単位行列かどうかを表わすフラグ
	 */
//	protected boolean m_fIdentity = false;
	public boolean m_fIdentity = false;
	
	//************************************************************************//
	//*** コンストラクタの定義
	//************************************************************************//
	
	/**
	 * 単位行列で初期化するコンストラクタ
	 */
	public CqMatrix(){
		super();
		Identity();
	}
	
//	public CqMatrix( int NoInit ){
//		
//	}
	
	/**
	 * スケールを指定するコンストラクタ<br>
	 * Scale matrix constructor
	 * @param xs	x軸のスケール
	 * @param ys	y軸のスケール
	 * @param zs	z軸のスケール
	 */
	public CqMatrix( float xs, float ys, float zs ){
		
	    Identity();

	    if ( xs == 1.0f && ys == 1.0f && zs == 1.0f )
	        m_fIdentity = true;
	    else
	    {
	    	m_aaElement[0][0] = xs;
	    	m_aaElement[1][1] = ys;
	    	m_aaElement[2][2] = zs;
	    	m_aaElement[3][3] = 1.0f;

	        m_fIdentity = false;
	    }
	}
	
	/**
	 * 平行移動を指定するコンストラクタ<br>
	 * @param Trans	平行移動を表わす３次元ベクトル
	 */
	public CqMatrix( CqVector3D Trans ){
		
		Identity();

	    if ( Trans.x == 0.0f && Trans.y == 0.0f && Trans.z == 0.0f )
	        m_fIdentity = true;
	    else
	    {
	        m_fIdentity = false;

	        m_aaElement[3][0] = Trans.x;
	        m_aaElement[3][1] = Trans.y;
	        m_aaElement[3][2] = Trans.z;
	    }
	}
	
	/**
	 * 軸周りの回転を指定するコンストラクタ<br>
	 * @param Angle	回転角（０〜２π）
	 * @param Axis		回転軸を表わすCqVector3Dオブジェクト
	 */
	public CqMatrix( float Angle, CqVector3D Axis ){
		
		Identity();
		
		if ( Angle != 0.0f && Axis.Magnitude() != 0.0f ){
			Rotate( Angle, Axis );
		}
		
		
	}
	
	/**
	 * 歪曲を指定するコンストラクタ<br>
	 * @param angle	角度（０〜２π）
	 * @param dx1		ベクトル１の第１要素
	 * @param dy1		ベクトル１の第２要素
	 * @param dz1		ベクトル１の第３要素
	 * @param dx2		ベクトル２の第１要素
	 * @param dy2		ベクトル２の第２要素
	 * @param dz2		ベクトル２の第３要素
	 */
	public CqMatrix( float angle, 
					  float dx1, float dy1, float dz1, 
					  float dx2, float dy2, float dz2 ){
		
	    // Normalize the two vectors, and construct a third perpendicular
	    CqVector3D d1 = new CqVector3D( dx1, dy1, dz1 );
	    CqVector3D d2 = new CqVector3D( dx2, dy2, dz2 );
	    d1.Unit();
	    d2.Unit();

	    // Assumes angle already changed to radians.

	    float d1d2dot = d1.mul(d2);
	    float axisangle = (float)acos( d1d2dot );
	    
	    if ( angle >= axisangle || angle <= ( axisangle - PI ) )
	    {
	        // Skewed past the axes -- issue error, then just use identity matrix.
	        // No access to CqBasicError from here, so this will have to be down
	        //   in RiSkew.  That would duplicate the above math calculations
	        //   unless they were passed to this constructor which would be odd
	        //   looking:
	        // CqBasicError(0,Severity_Normal,"RiSkew angle invalid.");
	        Identity();
	    }
	    else
	    {
	    	CqVector3D right = new CqVector3D();
	    	right.assignment( d1.mod(d2) );
	        right.Unit();

	        // d1ortho will be perpendicular to <d2> and <right> and can be
	        // used to construct a rotation matrix
	        CqVector3D d1ortho = new CqVector3D();
	        d1ortho.assignment( d2.mod(right) );

	        // 1) Rotate to a space where the skew operation is in a major plane.
	        // 2) Bend the y axis towards the z axis causing a skew.
	        // 3) Rotate back.
	        CqMatrix Rot = new CqMatrix(	right.x, d1ortho.x, d2.x, 0,
	                      					right.y, d1ortho.y, d2.y, 0,
	                      					right.z, d1ortho.z, d2.z, 0,
	                      					      0,         0,    0, 1 );
	        float par = d1d2dot;  // Amount of d1 parallel to d2
	        float perp = (float)(sqrt( 1.0 - par * par ));   // Amount perpendicular
	        float s = (float)(tan( angle + acos( perp ) ) * perp - par);
	        CqMatrix Skw = new CqMatrix(	1, 0, 0, 0,
	                      					0, 1, s, 0,
	                      					0, 0, 1, 0,
	                      					0, 0, 0, 1 );
	        
	        // Note the Inverse of a rotation matrix is its Transpose.
//	        this = Rot.Transpose() * Skw * Rot;
	        this.assignment(Rot.Transpose());
	        this.assignMul(Skw);
	        this.assignMul(Rot);
	        
//	        System.err.println( "-----01-----\n" + this);
//	        this.assignment(Rot.Transpose());
//	        System.err.println( "-----02-----\n" + this);
//	        this.assignMultiply(Skw);
//	        System.err.println( "-----03-----\n" + this);
//	        this.assignMultiply(Rot);
//	        System.err.println( "-----04-----\n" + this);
	        
	    }
	}
	
	/**
	 * コピーコンストラクタ
	 * @param from	コピー元
	 */
	public CqMatrix( CqMatrix from ){
		this.assignment(from);
		this.m_fIdentity = from.m_fIdentity;
	}
	
	/**
	 * 各要素を指定するコンストラクタ
	 * @param r1c1	１行１列目の要素
	 * @param r1c2	１行２列目の要素
	 * @param r1c3	１行３列目の要素
	 * @param r1c4	１行４列目の要素
	 * @param r2c1	２行１列目の要素
	 * @param r2c2	２行２列目の要素
	 * @param r2c3	２行３列目の要素
	 * @param r2c4	２行４列目の要素
	 * @param r3c1	３行１列目の要素
	 * @param r3c2	３行２列目の要素
	 * @param r3c3	３行３列目の要素
	 * @param r3c4	３行４列目の要素
	 * @param r4c1	４行１列目の要素
	 * @param r4c2	４行２列目の要素
	 * @param r4c3	４行３列目の要素
	 * @param r4c4	４行４列目の要素
	 */
	public CqMatrix(
			float r1c1, float r1c2, float r1c3, float r1c4, 
			float r2c1, float r2c2, float r2c3, float r2c4, 
			float r3c1, float r3c2, float r3c3, float r3c4,
			float r4c1, float r4c2, float r4c3, float r4c4 ){
        m_aaElement[ 0 ][ 0 ] = r1c1; m_aaElement[ 0 ][ 1 ] = r1c2; m_aaElement[ 0 ][ 2 ] = r1c3; m_aaElement[ 0 ][ 3 ] = r1c4;
        m_aaElement[ 1 ][ 0 ] = r2c1; m_aaElement[ 1 ][ 1 ] = r2c2; m_aaElement[ 1 ][ 2 ] = r2c3; m_aaElement[ 1 ][ 3 ] = r2c4;
        m_aaElement[ 2 ][ 0 ] = r3c1; m_aaElement[ 2 ][ 1 ] = r3c2; m_aaElement[ 2 ][ 2 ] = r3c3; m_aaElement[ 2 ][ 3 ] = r3c4;
        m_aaElement[ 3 ][ 0 ] = r4c1; m_aaElement[ 3 ][ 1 ] = r4c2; m_aaElement[ 3 ][ 2 ] = r4c3; m_aaElement[ 3 ][ 3 ] = r4c4;
        m_fIdentity = false;

	}
	
	/**
	 * float[4][4]で初期化するコンストラクタ
	 * @param From	4x4の２次元配列
	 */
	public CqMatrix( float From[][] ){
		m_aaElement[0][0] = From[0][0];	m_aaElement[0][1] = From[0][1];	m_aaElement[0][2] = From[0][2];	m_aaElement[0][3] = From[0][3];
		m_aaElement[1][0] = From[1][0];	m_aaElement[1][1] = From[1][1];	m_aaElement[1][2] = From[1][2];	m_aaElement[1][3] = From[1][3];
		m_aaElement[2][0] = From[2][0];	m_aaElement[2][1] = From[2][1];	m_aaElement[2][2] = From[2][2];	m_aaElement[2][3] = From[2][3];
		m_aaElement[3][0] = From[3][0];	m_aaElement[3][1] = From[3][1];	m_aaElement[3][2] = From[3][2];	m_aaElement[3][3] = From[3][3];
	}

	/**
	 * float[16]で初期化するコンストラクタ
	 * @param From　16要素の１次元配列
	 */
	public CqMatrix( float From[] ){
	    m_aaElement[ 0 ][ 0 ] = From[ 0 ];
	    m_aaElement[ 0 ][ 1 ] = From[ 1 ];
	    m_aaElement[ 0 ][ 2 ] = From[ 2 ];
	    m_aaElement[ 0 ][ 3 ] = From[ 3 ];
	    m_aaElement[ 1 ][ 0 ] = From[ 4 ];
	    m_aaElement[ 1 ][ 1 ] = From[ 5 ];
	    m_aaElement[ 1 ][ 2 ] = From[ 6 ];
	    m_aaElement[ 1 ][ 3 ] = From[ 7 ];
	    m_aaElement[ 2 ][ 0 ] = From[ 8 ];
	    m_aaElement[ 2 ][ 1 ] = From[ 9 ];
	    m_aaElement[ 2 ][ 2 ] = From[ 10 ];
	    m_aaElement[ 2 ][ 3 ] = From[ 11 ];
	    m_aaElement[ 3 ][ 0 ] = From[ 12 ];
	    m_aaElement[ 3 ][ 1 ] = From[ 13 ];
	    m_aaElement[ 3 ][ 2 ] = From[ 14 ];
	    m_aaElement[ 3 ][ 3 ] = From[ 15 ];
	    m_fIdentity = false;
	}
	
	/**
	 * すべての要素をfにするコンストラクタ
	 * @param f	すべての要素に代入する値
	 */
	public CqMatrix( float f ){
		m_aaElement[0][0] = m_aaElement[0][1] = m_aaElement[0][2] = m_aaElement[0][3] = f;
		m_aaElement[1][0] = m_aaElement[1][1] = m_aaElement[1][2] = m_aaElement[1][3] = f;
		m_aaElement[2][0] = m_aaElement[2][1] = m_aaElement[2][2] = m_aaElement[2][3] = f;
		m_aaElement[3][0] = m_aaElement[3][1] = m_aaElement[3][2] = m_aaElement[3][3] = f;
	}
	
	/**
	 * ４次元ベクトルをもとに平行移動を指定して初期化するコンストラクタ
	 * @param From	４次元ベクトル
	 */
	public CqMatrix( CqVector4D From ){
		this( new CqVector3D( From.x/From.w, From.y/From.w, From.z/From.w ) );
	}
	
	
	//************************************************************************//
	//*** メソッドの定義
	//************************************************************************//
	
	/**
	 * 単位化する
	 */
	public void Identity(){
	    m_aaElement[ 0 ][ 1 ] =
	        m_aaElement[ 0 ][ 2 ] =
	            m_aaElement[ 0 ][ 3 ] =
	                m_aaElement[ 1 ][ 0 ] =
	                    m_aaElement[ 1 ][ 2 ] =
	                        m_aaElement[ 1 ][ 3 ] =
	                            m_aaElement[ 2 ][ 0 ] =
	                                m_aaElement[ 2 ][ 1 ] =
	                                    m_aaElement[ 2 ][ 3 ] =
	                                        m_aaElement[ 3 ][ 0 ] =
	                                            m_aaElement[ 3 ][ 1 ] =
	                                                m_aaElement[ 3 ][ 2 ] = 0.0f;

	    m_aaElement[ 0 ][ 0 ] =
	        m_aaElement[ 1 ][ 1 ] =
	            m_aaElement[ 2 ][ 2 ] =
	                m_aaElement[ 3 ][ 3 ] = 1.0f;
	    m_fIdentity = true;
    
	}
	
    /**
     * 単位行列フラグを設定する。
     * 
     * @param f	単位行列かどうかを示すフラグ
     */
    public void SetfIdentity( boolean f )
    {
        m_fIdentity = f;
    }
	
	/**
	 * 単位行列かどうか 
	 * @return	true:単位行列である
	 */
	public boolean fIdentity(){
		return m_fIdentity;
	}
	
	/**
	 * スケールを設定する
	 * 
	 * @param S	スケール
	 */
	public void Scale( float S ){
	    if ( S != 1.0f ){
	    	Scale( S, S, S );
	    }
	}
	
	/**
	 * スケールを設定する
	 * 
	 * @param xs	Xスケール
	 * @param ys	Yスケール
	 * @param zs	Zスケール
	 */
	public void Scale( float xs, float ys, float zs ){
		
		CqMatrix Scale = new CqMatrix( xs, ys, zs );
		
		this.PreMultiply( Scale );
	}
	
	/**
	 * このCqMatrixを回転させる。
	 * 
	 * @param Angle	回転角（０〜２π）
	 * @param Axis		回転軸を表わすCqVector3Dオブジェクト
	 */
	public void Rotate( float Angle, CqVector3D Axis ){
		
		if ( Angle != 0.0f )
		{
			CqMatrix	R = new CqMatrix();
			R.Identity();
			CqVector3D	RotAxis = Axis;
			R.m_fIdentity = false;
			
			RotAxis.Unit();
			
			float	s = (float)( sin( Angle ) );
			float	c = (float)( cos( Angle ) );
			float	t = 1.0f - c;
			
			R.m_aaElement[0][0] = t * RotAxis.x * RotAxis.x + c;
			R.m_aaElement[1][1] = t * RotAxis.y * RotAxis.y + c;
			R.m_aaElement[2][2] = t * RotAxis.z * RotAxis.z + c;
			
			float	txy = t * RotAxis.x * RotAxis.y;
			float	sz = s * RotAxis.z;
			
			R.m_aaElement[0][1] = txy + sz;
			R.m_aaElement[1][0] = txy - sz;
			
			float	txz = t * RotAxis.x * RotAxis.z;
			float	sy = s * RotAxis.y;
			
			R.m_aaElement[0][2] = txz - sy;
			R.m_aaElement[2][0] = txz + sy;
			
			float	tyz = t * RotAxis.y * RotAxis.z;
			float	sx = s * RotAxis.x;
			
			R.m_aaElement[1][2] = tyz + sx;
			R.m_aaElement[2][1] = tyz - sx;
			
			this.PreMultiply( R );
		}
	}
	
	/**
	 * このCqMatrixを平行移動させる
	 * 
	 * @param Trans	平行移動を表わすCqVector3D
	 */
	public void Translate( CqVector3D Trans ){
	    CqMatrix matTrans = new CqMatrix( Trans );
	    this.PreMultiply( matTrans );
	}
	
	/**
	 * このCqMatrixを平行移動させる
	 * 
	 * @param xt	ｘ軸の平行移動
	 * @param yt	ｙ軸の平行移動
	 * @param zt	ｚ軸の平行移動
	 */
	public void Translate( float xt, float yt, float zt ){
	    if ( xt != 0.0f || yt != 0.0f || zt != 0.0f ){
	        Translate( new CqVector3D( xt, yt, zt ) );
	    }
	}
	
	/**
	 * シャーリング（せん断）を行う
	 * 
	 * @param yh	シャーリングのY要素
	 * @param zh	シャーリングのZ要素
	 */
	public void ShearX( float yh, float zh ){
	    CqMatrix Shear = new CqMatrix();
	    Shear.m_fIdentity = false;

	    Shear.m_aaElement[0][1] = yh;
	    Shear.m_aaElement[0][2] = zh;

	    this.PreMultiply( Shear );
	}
	
	/**
	 * シャーリング（せん断）を行う
	 * 
	 * @param xh	シャーリングのX要素
	 * @param zh	シャーリングのZ要素
	 */
	public void ShearY( float xh, float zh ){
	    CqMatrix Shear = new CqMatrix();;
	    Shear.m_fIdentity = false;

	    Shear.m_aaElement[1][0] = xh;
	    Shear.m_aaElement[1][2] = zh;

	    this.PreMultiply( Shear );
	}
	
	/**
	 * シャーリング（せん断）を行う
	 * 
	 * @param xh	シャーリングのX要素
	 * @param yh	シャーリングのY要素
	 */
	public void ShearZ( float xh, float yh ){
	    CqMatrix Shear = new CqMatrix();;
	    Shear.m_fIdentity = false;

	    Shear.m_aaElement[2][0] = xh;
	    Shear.m_aaElement[2][1] = yh;

	    this.PreMultiply( Shear );
	}
	
	/**
	 * 歪曲を行う（Skew） 
	 * 
	 * @param angle	角度（０〜２π）
	 * @param dx1		ベクトル１の第１要素
	 * @param dy1		ベクトル１の第２要素
	 * @param dz1		ベクトル１の第３要素
	 * @param dx2		ベクトル２の第１要素
	 * @param dy2		ベクトル２の第２要素
	 * @param dz2		ベクトル２の第３要素
	 */
	public void Skew( float angle,
            float dx1, float dy1, float dz1,
            float dx2, float dy2, float dz2 ){
		
	    CqMatrix Skew = new CqMatrix( angle, dx1, dy1, dz1, dx2, dy2, dz2 );

	    this.PreMultiply( Skew );
	}
	
	/**
	 * 正規化する
	 */
	public void Normalise(){
	    for ( int i = 0; i < 4; i++ )
	    {
	        for ( int j = 0; j < 4; j++ )
	        {
	            m_aaElement[ i ][ j ] /= m_aaElement[ 3 ][ 3 ];
//	        	this.setElement( i, j, getElement(i,j)/m33 );
	        }
	    }
	}
	
	
	
	/**
	 * 引数が右側にくる積（A' = AB）. <br>
	 * 自分自身に計算結果を代入する。
	 * 
	 * @param From	乗算するCqMatrix
	 * @return	計算結果（this）
	 */
	public CqMatrix PreMultiply( CqMatrix From ){
		
	    if ( m_fIdentity ){
	    	this.assignment( From );
	    	return this;
	    }
	    else if ( From.m_fIdentity ){
	        return ( this );
	    }
	    
	    CqMatrix A = new CqMatrix( this );

	    m_aaElement[ 0 ][ 0 ] = A.m_aaElement[ 0 ][ 0 ] * From.m_aaElement[ 0 ][ 0 ]
	                            + A.m_aaElement[ 0 ][ 1 ] * From.m_aaElement[ 1 ][ 0 ]
	                            + A.m_aaElement[ 0 ][ 2 ] * From.m_aaElement[ 2 ][ 0 ]
	                            + A.m_aaElement[ 0 ][ 3 ] * From.m_aaElement[ 3 ][ 0 ];
	    m_aaElement[ 0 ][ 1 ] = A.m_aaElement[ 0 ][ 0 ] * From.m_aaElement[ 0 ][ 1 ]
	                            + A.m_aaElement[ 0 ][ 1 ] * From.m_aaElement[ 1 ][ 1 ]
	                            + A.m_aaElement[ 0 ][ 2 ] * From.m_aaElement[ 2 ][ 1 ]
	                            + A.m_aaElement[ 0 ][ 3 ] * From.m_aaElement[ 3 ][ 1 ];
	    m_aaElement[ 0 ][ 2 ] = A.m_aaElement[ 0 ][ 0 ] * From.m_aaElement[ 0 ][ 2 ]
	                            + A.m_aaElement[ 0 ][ 1 ] * From.m_aaElement[ 1 ][ 2 ]
	                            + A.m_aaElement[ 0 ][ 2 ] * From.m_aaElement[ 2 ][ 2 ]
	                            + A.m_aaElement[ 0 ][ 3 ] * From.m_aaElement[ 3 ][ 2 ];
	    m_aaElement[ 0 ][ 3 ] = A.m_aaElement[ 0 ][ 0 ] * From.m_aaElement[ 0 ][ 3 ]
	                            + A.m_aaElement[ 0 ][ 1 ] * From.m_aaElement[ 1 ][ 3 ]
	                            + A.m_aaElement[ 0 ][ 2 ] * From.m_aaElement[ 2 ][ 3 ]
	                            + A.m_aaElement[ 0 ][ 3 ] * From.m_aaElement[ 3 ][ 3 ];

	    m_aaElement[ 1 ][ 0 ] = A.m_aaElement[ 1 ][ 0 ] * From.m_aaElement[ 0 ][ 0 ]
	                            + A.m_aaElement[ 1 ][ 1 ] * From.m_aaElement[ 1 ][ 0 ]
	                            + A.m_aaElement[ 1 ][ 2 ] * From.m_aaElement[ 2 ][ 0 ]
	                            + A.m_aaElement[ 1 ][ 3 ] * From.m_aaElement[ 3 ][ 0 ];
	    m_aaElement[ 1 ][ 1 ] = A.m_aaElement[ 1 ][ 0 ] * From.m_aaElement[ 0 ][ 1 ]
	                            + A.m_aaElement[ 1 ][ 1 ] * From.m_aaElement[ 1 ][ 1 ]
	                            + A.m_aaElement[ 1 ][ 2 ] * From.m_aaElement[ 2 ][ 1 ]
	                            + A.m_aaElement[ 1 ][ 3 ] * From.m_aaElement[ 3 ][ 1 ];
	    m_aaElement[ 1 ][ 2 ] = A.m_aaElement[ 1 ][ 0 ] * From.m_aaElement[ 0 ][ 2 ]
	                            + A.m_aaElement[ 1 ][ 1 ] * From.m_aaElement[ 1 ][ 2 ]
	                            + A.m_aaElement[ 1 ][ 2 ] * From.m_aaElement[ 2 ][ 2 ]
	                            + A.m_aaElement[ 1 ][ 3 ] * From.m_aaElement[ 3 ][ 2 ];
	    m_aaElement[ 1 ][ 3 ] = A.m_aaElement[ 1 ][ 0 ] * From.m_aaElement[ 0 ][ 3 ]
	                            + A.m_aaElement[ 1 ][ 1 ] * From.m_aaElement[ 1 ][ 3 ]
	                            + A.m_aaElement[ 1 ][ 2 ] * From.m_aaElement[ 2 ][ 3 ]
	                            + A.m_aaElement[ 1 ][ 3 ] * From.m_aaElement[ 3 ][ 3 ];

	    m_aaElement[ 2 ][ 0 ] = A.m_aaElement[ 2 ][ 0 ] * From.m_aaElement[ 0 ][ 0 ]
	                            + A.m_aaElement[ 2 ][ 1 ] * From.m_aaElement[ 1 ][ 0 ]
	                            + A.m_aaElement[ 2 ][ 2 ] * From.m_aaElement[ 2 ][ 0 ]
	                            + A.m_aaElement[ 2 ][ 3 ] * From.m_aaElement[ 3 ][ 0 ];
	    m_aaElement[ 2 ][ 1 ] = A.m_aaElement[ 2 ][ 0 ] * From.m_aaElement[ 0 ][ 1 ]
	                            + A.m_aaElement[ 2 ][ 1 ] * From.m_aaElement[ 1 ][ 1 ]
	                            + A.m_aaElement[ 2 ][ 2 ] * From.m_aaElement[ 2 ][ 1 ]
	                            + A.m_aaElement[ 2 ][ 3 ] * From.m_aaElement[ 3 ][ 1 ];
	    m_aaElement[ 2 ][ 2 ] = A.m_aaElement[ 2 ][ 0 ] * From.m_aaElement[ 0 ][ 2 ]
	                            + A.m_aaElement[ 2 ][ 1 ] * From.m_aaElement[ 1 ][ 2 ]
	                            + A.m_aaElement[ 2 ][ 2 ] * From.m_aaElement[ 2 ][ 2 ]
	                            + A.m_aaElement[ 2 ][ 3 ] * From.m_aaElement[ 3 ][ 2 ];
	    m_aaElement[ 2 ][ 3 ] = A.m_aaElement[ 2 ][ 0 ] * From.m_aaElement[ 0 ][ 3 ]
	                            + A.m_aaElement[ 2 ][ 1 ] * From.m_aaElement[ 1 ][ 3 ]
	                            + A.m_aaElement[ 2 ][ 2 ] * From.m_aaElement[ 2 ][ 3 ]
	                            + A.m_aaElement[ 2 ][ 3 ] * From.m_aaElement[ 3 ][ 3 ];

	    m_aaElement[ 3 ][ 0 ] = A.m_aaElement[ 3 ][ 0 ] * From.m_aaElement[ 0 ][ 0 ]
	                            + A.m_aaElement[ 3 ][ 1 ] * From.m_aaElement[ 1 ][ 0 ]
	                            + A.m_aaElement[ 3 ][ 2 ] * From.m_aaElement[ 2 ][ 0 ]
	                            + A.m_aaElement[ 3 ][ 3 ] * From.m_aaElement[ 3 ][ 0 ];
	    m_aaElement[ 3 ][ 1 ] = A.m_aaElement[ 3 ][ 0 ] * From.m_aaElement[ 0 ][ 1 ]
	                            + A.m_aaElement[ 3 ][ 1 ] * From.m_aaElement[ 1 ][ 1 ]
	                            + A.m_aaElement[ 3 ][ 2 ] * From.m_aaElement[ 2 ][ 1 ]
	                            + A.m_aaElement[ 3 ][ 3 ] * From.m_aaElement[ 3 ][ 1 ];
	    m_aaElement[ 3 ][ 2 ] = A.m_aaElement[ 3 ][ 0 ] * From.m_aaElement[ 0 ][ 2 ]
	                            + A.m_aaElement[ 3 ][ 1 ] * From.m_aaElement[ 1 ][ 2 ]
	                            + A.m_aaElement[ 3 ][ 2 ] * From.m_aaElement[ 2 ][ 2 ]
	                            + A.m_aaElement[ 3 ][ 3 ] * From.m_aaElement[ 3 ][ 2 ];
	    m_aaElement[ 3 ][ 3 ] = A.m_aaElement[ 3 ][ 0 ] * From.m_aaElement[ 0 ][ 3 ]
	                            + A.m_aaElement[ 3 ][ 1 ] * From.m_aaElement[ 1 ][ 3 ]
	                            + A.m_aaElement[ 3 ][ 2 ] * From.m_aaElement[ 2 ][ 3 ]
	                            + A.m_aaElement[ 3 ][ 3 ] * From.m_aaElement[ 3 ][ 3 ];

	    m_fIdentity = false;
	    return this;
	}
	
	/**
	 * 引数が右側にくる、４次元ベクトルとの積（v = Mv）<br>
	 * 
	 * @param Vector	乗算するCqVector4D
	 * @return	計算結果のCqVector4Dオブジェクト
	 */
	public CqVector4D PreMultiply( CqVector4D Vector ){
		
		if ( m_fIdentity ){
			return ( Vector );
		}
		
		CqVector4D Result = new CqVector4D();
		
	    Result.x( m_aaElement[ 0 ][ 0 ] * Vector.x
	              + m_aaElement[ 0 ][ 1 ] * Vector.y
	              + m_aaElement[ 0 ][ 2 ] * Vector.z
	              + m_aaElement[ 0 ][ 3 ] * Vector.w );

	    Result.y( m_aaElement[ 1 ][ 0 ] * Vector.x
	              + m_aaElement[ 1 ][ 1 ] * Vector.y
	              + m_aaElement[ 1 ][ 2 ] * Vector.z
	              + m_aaElement[ 1 ][ 3 ] * Vector.w );

	    Result.z( m_aaElement[ 2 ][ 0 ] * Vector.x
	              + m_aaElement[ 2 ][ 1 ] * Vector.y
	              + m_aaElement[ 2 ][ 2 ] * Vector.z
	              + m_aaElement[ 2 ][ 3 ] * Vector.w );

	    Result.h( m_aaElement[ 3 ][ 0 ] * Vector.x
	              + m_aaElement[ 3 ][ 1 ] * Vector.y
	              + m_aaElement[ 3 ][ 2 ] * Vector.z
	              + m_aaElement[ 3 ][ 3 ] * Vector.w );
		return Result;
	}
	
	/**
	 * 逆行列を返す。<br>
	 * ただし、この行列の行列式が０の場合、この行列自身を帰す。
	 * 
	 * @return	逆行列
	 */
	public CqMatrix Inverse(){
//		CqMatrix b = new CqMatrix(this);
////		b.invert();
////2007/05/14 watanabei Exceptionをキャッチするように変更
//		try{
//			b.invert();
//		}catch(SingularMatrixException sme){
//			HimawariLogger.outputException(sme);
//		}
////
		if(this.Determinant() == 0 )
			return this;
	    CqMatrix b = new CqMatrix();		// b evolves from identity into inverse(a)

	    if ( m_fIdentity )
	    {
	        b.assignment(this);
	    }
	    else
	    {
	        b.Identity();
	        b.m_fIdentity = false;

	        int i;
	        int j;
	        int i1;
	    //倍精度での計算に変更 2007/11/26
	    double[][] aa = new double[4][4];
	    double[][] bb = new double[4][4];
	    bb[0][0] = bb[1][1] = bb[2][2] = bb[3][3] = 1;

	    for(i = 0; i<4 ;i++){
	    	for(j=0; j <4 ;j++){
	    		aa[i][j] = this.m_aaElement[i][j];
	    	}
	    }
	        for ( j = 0; j < 4; j++ )    	// Find largest pivot in column j among rows j..3
	        {
	            i1 = j;
	            for ( i = j + 1; i < 4; i++ )
	            {
	                if ( Math.abs( aa[ i ][ j ] ) > Math.abs( aa[ i1 ][ j ] ) )
	                {
	                    i1 = i;
	                }
	            }

	            if ( i1 != j )
	            {
	                // Swap rows i1 and j in a and b to put pivot on diagonal
	                double temp;

	                temp = aa[ i1 ][ 0 ];
	                aa[ i1 ][ 0 ] = aa[ j ][ 0 ];
	                aa[ j ][ 0 ] = temp;
	                temp = aa[ i1 ][ 1 ];
	                aa[ i1 ][ 1 ] = aa[ j ][ 1 ];
	                aa[ j ][ 1 ] = temp;
	                temp = aa[ i1 ][ 2 ];
	                aa[ i1 ][ 2 ] = aa[ j ][ 2 ];
	                aa[ j ][ 2 ] = temp;
	                temp = aa[ i1 ][ 3 ];
	                aa[ i1 ][ 3 ] = aa[ j ][ 3 ];
	                aa[ j ][ 3 ] = temp;

	                temp = bb[ i1 ][ 0 ];
	                bb[ i1 ][ 0 ] = bb[ j ][ 0 ];
	                bb[ j ][ 0 ] = temp;
	                temp = bb[ i1 ][ 1 ];
	                bb[ i1 ][ 1 ] = bb[ j ][ 1 ];
	                bb[ j ][ 1 ] = temp;
	                temp = bb[ i1 ][ 2 ];
	                bb[ i1 ][ 2 ] = bb[ j ][ 2 ];
	                bb[ j ][ 2 ] = temp;
	                temp = bb[ i1 ][ 3 ];
	                bb[ i1 ][ 3 ] = bb[ j ][ 3 ];
	                bb[ j ][ 3 ] = temp;
	            }

	            // Scale row j to have a unit diagonal
	            if ( aa[ j ][ j ] == 0.0f )
	            {
	                // Can't invert a singular matrix!
	                //				throw XssMatrix(MatrixErr_Singular, __LINE__, ssMakeWide(__FILE__));
	            }
	            double scale = 1.0f / aa[ j ][ j ];
	            bb[ j ][ 0 ] *= scale;
	            bb[ j ][ 1 ] *= scale;
	            bb[ j ][ 2 ] *= scale;
	            bb[ j ][ 3 ] *= scale;
	            // all elements to left of a[j][j] are already zero
	            for ( i1 = j + 1; i1 < 4; i1++ )
	            {
	                aa[ j ][ i1 ] *= scale;
	            }
	            aa[ j ][ j ] = 1.0f;

	            // Eliminate off-diagonal elements in column j of a, doing identical ops to b
	            for ( i = 0; i < 4; i++ )
	            {
	                if ( i != j )
	                {
	                    scale = aa[ i ][ j ];
	                    bb[ i ][ 0 ] -= scale * bb[ j ][ 0 ];
	                    bb[ i ][ 1 ] -= scale * bb[ j ][ 1 ];
	                    bb[ i ][ 2 ] -= scale * bb[ j ][ 2 ];
	                    bb[ i ][ 3 ] -= scale * bb[ j ][ 3 ];

	                    // all elements to left of a[j][j] are zero
	                    // a[j][j] is 1.0
	                    for ( i1 = j + 1; i1 < 4; i1++ )
	                    {
	                        aa[ i ][ i1 ] -= scale * aa[ j ][ i1 ];
	                    }
	                    aa[ i ][ j ] = 0.0f;
	                }
	            }
	        }
	        for(i = 0; i < 4 ;i++){
	        	for(j = 0 ; j < 4 ; j++){
	        		b.m_aaElement[i][j] =(float) bb[i][j];
	        	}
	        }
	    }//ここまで
		return b;
	}

	/**
	 * 転置行列を返す
	 * 
	 * @return	この行列の転置行列
	 */
	public CqMatrix Transpose(){
		CqMatrix Temp;
		
	    if ( m_fIdentity )
	    {
	        Temp = new CqMatrix(this);
	    }
	    else{
	    	Temp = new CqMatrix(this);
	        Temp.m_aaElement[ 0 ][ 0 ] = m_aaElement[ 0 ][ 0 ];
	        Temp.m_aaElement[ 0 ][ 1 ] = m_aaElement[ 1 ][ 0 ];
	        Temp.m_aaElement[ 0 ][ 2 ] = m_aaElement[ 2 ][ 0 ];
	        Temp.m_aaElement[ 0 ][ 3 ] = m_aaElement[ 3 ][ 0 ];
	        Temp.m_aaElement[ 1 ][ 0 ] = m_aaElement[ 0 ][ 1 ];
	        Temp.m_aaElement[ 1 ][ 1 ] = m_aaElement[ 1 ][ 1 ];
	        Temp.m_aaElement[ 1 ][ 2 ] = m_aaElement[ 2 ][ 1 ];
	        Temp.m_aaElement[ 1 ][ 3 ] = m_aaElement[ 3 ][ 1 ];
	        Temp.m_aaElement[ 2 ][ 0 ] = m_aaElement[ 0 ][ 2 ];
	        Temp.m_aaElement[ 2 ][ 1 ] = m_aaElement[ 1 ][ 2 ];
	        Temp.m_aaElement[ 2 ][ 2 ] = m_aaElement[ 2 ][ 2 ];
	        Temp.m_aaElement[ 2 ][ 3 ] = m_aaElement[ 3 ][ 2 ];
	        Temp.m_aaElement[ 3 ][ 0 ] = m_aaElement[ 0 ][ 3 ];
	        Temp.m_aaElement[ 3 ][ 1 ] = m_aaElement[ 1 ][ 3 ];
	        Temp.m_aaElement[ 3 ][ 2 ] = m_aaElement[ 2 ][ 3 ];
	        Temp.m_aaElement[ 3 ][ 3 ] = m_aaElement[ 3 ][ 3 ];

	        Temp.m_fIdentity = false;
	    }
		
		return Temp;
	}
	
	/**
	 * 行列式の演算
	 * 
	 * @return	行列式
	 */
	public float Determinant(){
	    // Assign to individual variable names to aid selecting correct elements
	    float a1 = m_aaElement[ 0 ][ 0 ];
	    float b1 = m_aaElement[ 0 ][ 1 ];
	    float c1 = m_aaElement[ 0 ][ 2 ];
	    float d1 = m_aaElement[ 0 ][ 3 ];

	    float a2 = m_aaElement[ 1 ][ 0 ];
	    float b2 = m_aaElement[ 1 ][ 1 ];
	    float c2 = m_aaElement[ 1 ][ 2 ];
	    float d2 = m_aaElement[ 1 ][ 3 ];

	    float a3 = m_aaElement[ 2 ][ 0 ];
	    float b3 = m_aaElement[ 2 ][ 1 ];
	    float c3 = m_aaElement[ 2 ][ 2 ];
	    float d3 = m_aaElement[ 2 ][ 3 ];

	    float a4 = m_aaElement[ 3 ][ 0 ];
	    float b4 = m_aaElement[ 3 ][ 1 ];
	    float c4 = m_aaElement[ 3 ][ 2 ];
	    float d4 = m_aaElement[ 3 ][ 3 ];

	    return a1 * det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4 ) -
	           b1 * det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4 ) +
	           c1 * det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4 ) -
	           d1 * det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4 );

	}
	
//	public float Element( int iRow, int iColumn )
//    {
//        return ( m_aaElement[ iRow ][ iColumn ] );
//    }
//	
//    void	SetElement( int iRow, int iColumn, float fValue )
//    {
//        m_aaElement[ iRow ][ iColumn ] = fValue;
//    }
	
	//************************************************************************//
	//*** 演算子のオーバーロード
	//************************************************************************//
	
	/**
	 * 乗算演算子。
	 * 行列の積を返す。引数が左側にくる乗算の計算（M = BA）
	 * 
	 * @param From	乗算するCqMatrix
	 * @return	計算結果
	 */
	public CqMatrix multiply( final CqMatrix From ){
		
	    CqMatrix Temp = new CqMatrix(this);
	    Temp.assignMul( From );
	    
	    return Temp ;
	}
	
	/**
	 * 乗算代入。
	 * 行列の積を自分自身に代入する。引数が左側にくる乗算の計算（A' = BA）
	 * 
	 * @param From	乗算するCqMatrix
	 * @return	計算結果（this）
	 */
	public CqMatrix assignMul( final CqMatrix From ){
		
	    if ( m_fIdentity ){
	    	this.assignment( From );
	    	return this;
	    }
	    else if ( From.m_fIdentity ){
	        return ( this );
	    }
	    
	    CqMatrix A = new CqMatrix(this );

	    m_aaElement[ 0 ][ 0 ] = From.m_aaElement[ 0 ][ 0 ] * A.m_aaElement[ 0 ][ 0 ]
	                            + From.m_aaElement[ 0 ][ 1 ] * A.m_aaElement[ 1 ][ 0 ]
	                            + From.m_aaElement[ 0 ][ 2 ] * A.m_aaElement[ 2 ][ 0 ]
	                            + From.m_aaElement[ 0 ][ 3 ] * A.m_aaElement[ 3 ][ 0 ];
	    m_aaElement[ 0 ][ 1 ] = From.m_aaElement[ 0 ][ 0 ] * A.m_aaElement[ 0 ][ 1 ]
	                            + From.m_aaElement[ 0 ][ 1 ] * A.m_aaElement[ 1 ][ 1 ]
	                            + From.m_aaElement[ 0 ][ 2 ] * A.m_aaElement[ 2 ][ 1 ]
	                            + From.m_aaElement[ 0 ][ 3 ] * A.m_aaElement[ 3 ][ 1 ];
	    m_aaElement[ 0 ][ 2 ] = From.m_aaElement[ 0 ][ 0 ] * A.m_aaElement[ 0 ][ 2 ]
	                            + From.m_aaElement[ 0 ][ 1 ] * A.m_aaElement[ 1 ][ 2 ]
	                            + From.m_aaElement[ 0 ][ 2 ] * A.m_aaElement[ 2 ][ 2 ]
	                            + From.m_aaElement[ 0 ][ 3 ] * A.m_aaElement[ 3 ][ 2 ];
	    m_aaElement[ 0 ][ 3 ] = From.m_aaElement[ 0 ][ 0 ] * A.m_aaElement[ 0 ][ 3 ]
	                            + From.m_aaElement[ 0 ][ 1 ] * A.m_aaElement[ 1 ][ 3 ]
	                            + From.m_aaElement[ 0 ][ 2 ] * A.m_aaElement[ 2 ][ 3 ]
	                            + From.m_aaElement[ 0 ][ 3 ] * A.m_aaElement[ 3 ][ 3 ];

	    m_aaElement[ 1 ][ 0 ] = From.m_aaElement[ 1 ][ 0 ] * A.m_aaElement[ 0 ][ 0 ]
	                            + From.m_aaElement[ 1 ][ 1 ] * A.m_aaElement[ 1 ][ 0 ]
	                            + From.m_aaElement[ 1 ][ 2 ] * A.m_aaElement[ 2 ][ 0 ]
	                            + From.m_aaElement[ 1 ][ 3 ] * A.m_aaElement[ 3 ][ 0 ];
	    m_aaElement[ 1 ][ 1 ] = From.m_aaElement[ 1 ][ 0 ] * A.m_aaElement[ 0 ][ 1 ]
	                            + From.m_aaElement[ 1 ][ 1 ] * A.m_aaElement[ 1 ][ 1 ]
	                            + From.m_aaElement[ 1 ][ 2 ] * A.m_aaElement[ 2 ][ 1 ]
	                            + From.m_aaElement[ 1 ][ 3 ] * A.m_aaElement[ 3 ][ 1 ];
	    m_aaElement[ 1 ][ 2 ] = From.m_aaElement[ 1 ][ 0 ] * A.m_aaElement[ 0 ][ 2 ]
	                            + From.m_aaElement[ 1 ][ 1 ] * A.m_aaElement[ 1 ][ 2 ]
	                            + From.m_aaElement[ 1 ][ 2 ] * A.m_aaElement[ 2 ][ 2 ]
	                            + From.m_aaElement[ 1 ][ 3 ] * A.m_aaElement[ 3 ][ 2 ];
	    m_aaElement[ 1 ][ 3 ] = From.m_aaElement[ 1 ][ 0 ] * A.m_aaElement[ 0 ][ 3 ]
	                            + From.m_aaElement[ 1 ][ 1 ] * A.m_aaElement[ 1 ][ 3 ]
	                            + From.m_aaElement[ 1 ][ 2 ] * A.m_aaElement[ 2 ][ 3 ]
	                            + From.m_aaElement[ 1 ][ 3 ] * A.m_aaElement[ 3 ][ 3 ];

	    m_aaElement[ 2 ][ 0 ] = From.m_aaElement[ 2 ][ 0 ] * A.m_aaElement[ 0 ][ 0 ]
	                            + From.m_aaElement[ 2 ][ 1 ] * A.m_aaElement[ 1 ][ 0 ]
	                            + From.m_aaElement[ 2 ][ 2 ] * A.m_aaElement[ 2 ][ 0 ]
	                            + From.m_aaElement[ 2 ][ 3 ] * A.m_aaElement[ 3 ][ 0 ];
	    m_aaElement[ 2 ][ 1 ] = From.m_aaElement[ 2 ][ 0 ] * A.m_aaElement[ 0 ][ 1 ]
	                            + From.m_aaElement[ 2 ][ 1 ] * A.m_aaElement[ 1 ][ 1 ]
	                            + From.m_aaElement[ 2 ][ 2 ] * A.m_aaElement[ 2 ][ 1 ]
	                            + From.m_aaElement[ 2 ][ 3 ] * A.m_aaElement[ 3 ][ 1 ];
	    m_aaElement[ 2 ][ 2 ] = From.m_aaElement[ 2 ][ 0 ] * A.m_aaElement[ 0 ][ 2 ]
	                            + From.m_aaElement[ 2 ][ 1 ] * A.m_aaElement[ 1 ][ 2 ]
	                            + From.m_aaElement[ 2 ][ 2 ] * A.m_aaElement[ 2 ][ 2 ]
	                            + From.m_aaElement[ 2 ][ 3 ] * A.m_aaElement[ 3 ][ 2 ];
	    m_aaElement[ 2 ][ 3 ] = From.m_aaElement[ 2 ][ 0 ] * A.m_aaElement[ 0 ][ 3 ]
	                            + From.m_aaElement[ 2 ][ 1 ] * A.m_aaElement[ 1 ][ 3 ]
	                            + From.m_aaElement[ 2 ][ 2 ] * A.m_aaElement[ 2 ][ 3 ]
	                            + From.m_aaElement[ 2 ][ 3 ] * A.m_aaElement[ 3 ][ 3 ];

	    m_aaElement[ 3 ][ 0 ] = From.m_aaElement[ 3 ][ 0 ] * A.m_aaElement[ 0 ][ 0 ]
	                            + From.m_aaElement[ 3 ][ 1 ] * A.m_aaElement[ 1 ][ 0 ]
	                            + From.m_aaElement[ 3 ][ 2 ] * A.m_aaElement[ 2 ][ 0 ]
	                            + From.m_aaElement[ 3 ][ 3 ] * A.m_aaElement[ 3 ][ 0 ];
	    m_aaElement[ 3 ][ 1 ] = From.m_aaElement[ 3 ][ 0 ] * A.m_aaElement[ 0 ][ 1 ]
	                            + From.m_aaElement[ 3 ][ 1 ] * A.m_aaElement[ 1 ][ 1 ]
	                            + From.m_aaElement[ 3 ][ 2 ] * A.m_aaElement[ 2 ][ 1 ]
	                            + From.m_aaElement[ 3 ][ 3 ] * A.m_aaElement[ 3 ][ 1 ];
	    m_aaElement[ 3 ][ 2 ] = From.m_aaElement[ 3 ][ 0 ] * A.m_aaElement[ 0 ][ 2 ]
	                            + From.m_aaElement[ 3 ][ 1 ] * A.m_aaElement[ 1 ][ 2 ]
	                            + From.m_aaElement[ 3 ][ 2 ] * A.m_aaElement[ 2 ][ 2 ]
	                            + From.m_aaElement[ 3 ][ 3 ] * A.m_aaElement[ 3 ][ 2 ];
	    m_aaElement[ 3 ][ 3 ] = From.m_aaElement[ 3 ][ 0 ] * A.m_aaElement[ 0 ][ 3 ]
	                            + From.m_aaElement[ 3 ][ 1 ] * A.m_aaElement[ 1 ][ 3 ]
	                            + From.m_aaElement[ 3 ][ 2 ] * A.m_aaElement[ 2 ][ 3 ]
	                            + From.m_aaElement[ 3 ][ 3 ] * A.m_aaElement[ 3 ][ 3 ];

	    m_fIdentity = false;
	    
	    return this;
	}
	
	/**
	 * 乗算演算子。
	 * 引数値のスケールの行列を作成し、それとの積を返す。
	 * 
	 * @param S	スケール
	 * @return	計算結果
	 */
	public CqMatrix multiply( final float S ){
		
	    CqMatrix Temp = new CqMatrix(this);
	    Temp.assignMul(S);
	    
	    return Temp ;
	}
	
	/**
	 * 乗算演算子。
	 * 引数値のスケールの行列を作成し、それとの積を自分自身に代入する。
	 * 
	 * @param S	スケール
	 * @return	計算結果（this)
	 */
	public CqMatrix assignMul( final float S  ){

	    CqMatrix ScaleMatrix = new CqMatrix( S, S, S );

	    this.PreMultiply( ScaleMatrix );

	    return this;
	}
	
	/**
	 * 乗算演算子。
	 * 1x4のベクトルを左から掛けた積を計算返す（v'=vM）
	 * 
	 * @param Vector	乗算するCqVector4D
	 * @return	計算結果のCqVector4D（1x4ベクトル）
	 */
	public 	CqVector4D multiply( final CqVector4D Vector ){
		
	    if ( m_fIdentity ){
	        return ( Vector );
	    }

	    CqVector4D	Result = new CqVector4D();

	    Result.x =( m_aaElement[0][0] * Vector.x
	              + m_aaElement[1][0] * Vector.y
	              + m_aaElement[2][0] * Vector.z
	              + m_aaElement[3][0] * Vector.w );

	    Result.y =( m_aaElement[0][1] * Vector.x
	              + m_aaElement[1][1] * Vector.y
	              + m_aaElement[2][1] * Vector.z
	              + m_aaElement[3][1] * Vector.w );

	    Result.z =( m_aaElement[0][2] * Vector.x
	              + m_aaElement[1][2] * Vector.y
	              + m_aaElement[2][2] * Vector.z
	              + m_aaElement[3][2] * Vector.w );

	    Result.w =( m_aaElement[0][3] * Vector.x
	              + m_aaElement[1][3] * Vector.y
	              + m_aaElement[2][3] * Vector.z
	              + m_aaElement[3][3] * Vector.w );

	    return ( Result );
	}
	
	/**
	 * 乗算演算子。
	 * ３次元ベクトルとの積を返す。
	 * 
	 * @param Vector	乗算するCqVector3D
	 * @return		計算結果のCqVector3D
	 */
	public CqVector3D multiply( final CqVector3D Vector )
	{
	    if ( m_fIdentity ){
	        return ( Vector );
	    }

	    CqVector3D	Result = new CqVector3D();

	    float h = ( m_aaElement[0][3] * Vector.x
	                  + m_aaElement[1][3] * Vector.y
	                  + m_aaElement[2][3] * Vector.z
	                  + m_aaElement[3][3] );

	    Result.x =  ( m_aaElement[0][0] * Vector.x
	                + m_aaElement[1][0] * Vector.y
	                + m_aaElement[2][0] * Vector.z
	                + m_aaElement[3][0] ) / h;

	    Result.y =  ( m_aaElement[0][1] * Vector.x
	                + m_aaElement[1][1] * Vector.y
	                + m_aaElement[2][1] * Vector.z
	                + m_aaElement[3][1] ) / h;

	    Result.z =  ( m_aaElement[0][2] * Vector.x
	                + m_aaElement[1][2] * Vector.y
	                + m_aaElement[2][2] * Vector.z
	                + m_aaElement[3][2] ) / h;


	    return ( Result );
	}
	
	/**
	 * 加算演算子。
	 * ４次元ベクトルをもとに平行移動の３次元ベクトルを求め、自分自身との積を返す。
	 * 
	 * @param Vector	加算する４次元ベクトル
	 * @return	計算結果
	 */
	public CqMatrix add( final CqVector4D Vector )
	{
	    CqMatrix Temp = new CqMatrix(this);
	    Temp.assignAdd( Vector );
	    return ( Temp );
	}
	
	/**
	 * 加算代入。
	 * ４次元ベクトルをもとに平行移動の３次元ベクトルを求め、自分自身との積を返す。
	 * 
	 * @param Vector	加算する４次元ベクトル
	 * @return	計算結果（this）
	 */
	public CqMatrix assignAdd( final CqVector4D Vector )
	{
	    CqMatrix Trans = new CqMatrix( Vector );

	    this.PreMultiply( Trans );

	    return ( this );
	}
	
	/**
	 * 減算演算子。
	 * ４次元ベクトルをもとに平行移動の３次元ベクトルを求め、自分自身との積を返す。<br>
	 * その際、第１〜３要素の符号を逆にして求める。
	 * 
	 * @param Vector	減算する４次元ベクトル
	 * @return	計算結果
	 */
	public CqMatrix sub( final CqVector4D Vector )
	{
	    CqMatrix Temp = new CqMatrix( this );
	    Temp.assignSub( Vector );
	    return ( Temp );
	}

	/**
	 * 減算代入。
	 * ４次元ベクトルをもとに平行移動の３次元ベクトルを求め、自分自身との積を返す。<br>
	 * その際、第１〜３要素の符号を逆にして求める。
	 * 
	 * @param Vector	減算する４次元ベクトル
	 * @return	計算結果（this）
	 */
	public CqMatrix assignSub( final CqVector4D Vector )
	{
	    CqVector4D Temp = new CqVector4D( Vector );

	    Temp.x = -Temp.x;
	    Temp.y = -Temp.y;
	    Temp.z = -Temp.z;
	    
	    CqMatrix Trans = new CqMatrix( Temp );

	    this.PreMultiply( Trans );

	    return ( this );
	}
	
	
	/**
	 * 加算演算子。
	 * 4x4行列同士の足し算 
	 * 
	 * @param From	加算するCqMatrix
	 * @return	計算結果
	 */
	public CqMatrix add( final CqMatrix From )
	{
	    CqMatrix Temp = new CqMatrix( this );
	    Temp.assignAdd( From );
	    return ( Temp );
	}
	

	/**
	 * 加算代入。
	 * 4x4同士を加算し、計算結果を自分自身とする。
	 * 
	 * @param From	加算するCqMatrix
	 * @return	計算結果（this）
	 */
	public CqMatrix assignAdd( final CqMatrix From )
	{
	    m_aaElement[ 0 ][ 0 ] += From.m_aaElement[ 0 ][ 0 ];
	    m_aaElement[ 1 ][ 0 ] += From.m_aaElement[ 1 ][ 0 ];
	    m_aaElement[ 2 ][ 0 ] += From.m_aaElement[ 2 ][ 0 ];
	    m_aaElement[ 3 ][ 0 ] += From.m_aaElement[ 3 ][ 0 ];
	    m_aaElement[ 0 ][ 1 ] += From.m_aaElement[ 0 ][ 1 ];
	    m_aaElement[ 1 ][ 1 ] += From.m_aaElement[ 1 ][ 1 ];
	    m_aaElement[ 2 ][ 1 ] += From.m_aaElement[ 2 ][ 1 ];
	    m_aaElement[ 3 ][ 1 ] += From.m_aaElement[ 3 ][ 1 ];
	    m_aaElement[ 0 ][ 2 ] += From.m_aaElement[ 0 ][ 2 ];
	    m_aaElement[ 1 ][ 2 ] += From.m_aaElement[ 1 ][ 2 ];
	    m_aaElement[ 2 ][ 2 ] += From.m_aaElement[ 2 ][ 2 ];
	    m_aaElement[ 3 ][ 2 ] += From.m_aaElement[ 3 ][ 2 ];
	    m_aaElement[ 0 ][ 3 ] += From.m_aaElement[ 0 ][ 3 ];
	    m_aaElement[ 1 ][ 3 ] += From.m_aaElement[ 1 ][ 3 ];
	    m_aaElement[ 2 ][ 3 ] += From.m_aaElement[ 2 ][ 3 ];
	    m_aaElement[ 3 ][ 3 ] += From.m_aaElement[ 3 ][ 3 ];
		    
	    m_fIdentity = false;

	    return ( this );
	}
	
	/**
	 * 減算演算子。
	 * 4x4行列同士の足し算 
	 * 
	 * @param From	減算するCqMatrix
	 * @return	計算結果
	 */
	public CqMatrix sub( final CqMatrix From )
	{
	    CqMatrix Temp = new CqMatrix( this );
	    Temp.assignSub( From );
	    return ( Temp );
	}
	
	/**
	 * 減算代入。
	 * 4x4同士を減算し、計算結果を自分自身とする。
	 * 
	 * @param From	減算するCqMatrix
	 * @return	計算結果（this）
	 */
	public CqMatrix assignSub( final CqMatrix From )
	{
	    m_aaElement[ 0 ][ 0 ] -= From.m_aaElement[ 0 ][ 0 ];
	    m_aaElement[ 1 ][ 0 ] -= From.m_aaElement[ 1 ][ 0 ];
	    m_aaElement[ 2 ][ 0 ] -= From.m_aaElement[ 2 ][ 0 ];
	    m_aaElement[ 3 ][ 0 ] -= From.m_aaElement[ 3 ][ 0 ];
	    m_aaElement[ 0 ][ 1 ] -= From.m_aaElement[ 0 ][ 1 ];
	    m_aaElement[ 1 ][ 1 ] -= From.m_aaElement[ 1 ][ 1 ];
	    m_aaElement[ 2 ][ 1 ] -= From.m_aaElement[ 2 ][ 1 ];
	    m_aaElement[ 3 ][ 1 ] -= From.m_aaElement[ 3 ][ 1 ];
	    m_aaElement[ 0 ][ 2 ] -= From.m_aaElement[ 0 ][ 2 ];
	    m_aaElement[ 1 ][ 2 ] -= From.m_aaElement[ 1 ][ 2 ];
	    m_aaElement[ 2 ][ 2 ] -= From.m_aaElement[ 2 ][ 2 ];
	    m_aaElement[ 3 ][ 2 ] -= From.m_aaElement[ 3 ][ 2 ];
	    m_aaElement[ 0 ][ 3 ] -= From.m_aaElement[ 0 ][ 3 ];
	    m_aaElement[ 1 ][ 3 ] -= From.m_aaElement[ 1 ][ 3 ];
	    m_aaElement[ 2 ][ 3 ] -= From.m_aaElement[ 2 ][ 3 ];
	    m_aaElement[ 3 ][ 3 ] -= From.m_aaElement[ 3 ][ 3 ];
		
	    m_fIdentity = false;

	    return ( this );
	}
		
	/**
	 * 代入演算子。
	 * 引数の行列を自分自身とする。
	 * 
	 * @param From	代入するCqMatrix
	 * @return	計算結果（this）
	 */
	public CqMatrix assignment( final CqMatrix From )
	{
	    m_aaElement[ 0 ][ 0 ] = From.m_aaElement[ 0 ][ 0 ];
	    m_aaElement[ 1 ][ 0 ] = From.m_aaElement[ 1 ][ 0 ];
	    m_aaElement[ 2 ][ 0 ] = From.m_aaElement[ 2 ][ 0 ];
	    m_aaElement[ 3 ][ 0 ] = From.m_aaElement[ 3 ][ 0 ];
	    m_aaElement[ 0 ][ 1 ] = From.m_aaElement[ 0 ][ 1 ];
	    m_aaElement[ 1 ][ 1 ] = From.m_aaElement[ 1 ][ 1 ];
	    m_aaElement[ 2 ][ 1 ] = From.m_aaElement[ 2 ][ 1 ];
	    m_aaElement[ 3 ][ 1 ] = From.m_aaElement[ 3 ][ 1 ];
	    m_aaElement[ 0 ][ 2 ] = From.m_aaElement[ 0 ][ 2 ];
	    m_aaElement[ 1 ][ 2 ] = From.m_aaElement[ 1 ][ 2 ];
	    m_aaElement[ 2 ][ 2 ] = From.m_aaElement[ 2 ][ 2 ];
	    m_aaElement[ 3 ][ 2 ] = From.m_aaElement[ 3 ][ 2 ];
	    m_aaElement[ 0 ][ 3 ] = From.m_aaElement[ 0 ][ 3 ];
	    m_aaElement[ 1 ][ 3 ] = From.m_aaElement[ 1 ][ 3 ];
	    m_aaElement[ 2 ][ 3 ] = From.m_aaElement[ 2 ][ 3 ];
	    m_aaElement[ 3 ][ 3 ] = From.m_aaElement[ 3 ][ 3 ];

		this.m_fIdentity = From.m_fIdentity;
		return this;
	}
	
	/**
	 * 代入演算子。
	 * 4x4のfloat配列を自分自身とする。
	 * 
	 * @param From	代入するfloat[4][4]
	 * @return	計算結果（this）
	 */
	public CqMatrix assignment( float[][] From )
	{
	    m_aaElement[ 0 ][ 0 ] = From[ 0 ][ 0 ];
	    m_aaElement[ 1 ][ 0 ] = From[ 1 ][ 0 ];
	    m_aaElement[ 2 ][ 0 ] = From[ 2 ][ 0 ];
	    m_aaElement[ 3 ][ 0 ] = From[ 3 ][ 0 ];
	    m_aaElement[ 0 ][ 1 ] = From[ 0 ][ 1 ];
	    m_aaElement[ 1 ][ 1 ] = From[ 1 ][ 1 ];
	    m_aaElement[ 2 ][ 1 ] = From[ 2 ][ 1 ];
	    m_aaElement[ 3 ][ 1 ] = From[ 3 ][ 1 ];
	    m_aaElement[ 0 ][ 2 ] = From[ 0 ][ 2 ];
	    m_aaElement[ 1 ][ 2 ] = From[ 1 ][ 2 ];
	    m_aaElement[ 2 ][ 2 ] = From[ 2 ][ 2 ];
	    m_aaElement[ 3 ][ 2 ] = From[ 3 ][ 2 ];
	    m_aaElement[ 0 ][ 3 ] = From[ 0 ][ 3 ];
	    m_aaElement[ 1 ][ 3 ] = From[ 1 ][ 3 ];
	    m_aaElement[ 2 ][ 3 ] = From[ 2 ][ 3 ];
	    m_aaElement[ 3 ][ 3 ] = From[ 3 ][ 3 ];
		
		this.m_fIdentity = false;
		return this;
	}
	
	
	/**
	 * 代入演算子。
	 * 1x16のfloat配列を自分自身とする。
	 * 
	 * @param From	代入するfloat[16]
	 * @return	計算結果（this）
	 */
	public CqMatrix assignment( float[] From )
	{
	    m_aaElement[ 0 ][ 0 ] = From[ 0 ];
	    m_aaElement[ 0 ][ 1 ] = From[ 1 ];
	    m_aaElement[ 0 ][ 2 ] = From[ 2 ];
	    m_aaElement[ 0 ][ 3 ] = From[ 3 ];
	    m_aaElement[ 1 ][ 0 ] = From[ 4 ];
	    m_aaElement[ 1 ][ 1 ] = From[ 5 ];
	    m_aaElement[ 1 ][ 2 ] = From[ 6 ];
	    m_aaElement[ 1 ][ 3 ] = From[ 7 ];
	    m_aaElement[ 2 ][ 0 ] = From[ 8 ];
	    m_aaElement[ 2 ][ 1 ] = From[ 9 ];
	    m_aaElement[ 2 ][ 2 ] = From[ 10 ];
	    m_aaElement[ 2 ][ 3 ] = From[ 11 ];
	    m_aaElement[ 3 ][ 0 ] = From[ 12 ];
	    m_aaElement[ 3 ][ 1 ] = From[ 13 ];
	    m_aaElement[ 3 ][ 2 ] = From[ 14 ];
	    m_aaElement[ 3 ][ 3 ] = From[ 15 ];

		
		this.m_fIdentity = false;
		return this;
	}
	

	
	/**
	 * 逆乗算演算子。
	 * 左側からスカラを掛けた場合の計算。<br>
	 * スカラとの乗算は、左側から掛けるとスカラ倍を、右側からだとScaleを表わす。
	 * 
	 * @param S	スカラ値
	 * @return	計算結果
	 */
	public CqMatrix mulInv( float S )
	{
	    CqMatrix Temp = new CqMatrix( this );
	    Temp.m_aaElement[ 0 ][ 0 ] *= S;
	    Temp.m_aaElement[ 1 ][ 0 ] *= S;
	    Temp.m_aaElement[ 2 ][ 0 ] *= S;
	    Temp.m_aaElement[ 3 ][ 0 ] *= S;

	    Temp.m_aaElement[ 0 ][ 1 ] *= S;
	    Temp.m_aaElement[ 1 ][ 1 ] *= S;
	    Temp.m_aaElement[ 2 ][ 1 ] *= S;
	    Temp.m_aaElement[ 3 ][ 1 ] *= S;

	    Temp.m_aaElement[ 0 ][ 2 ] *= S;
	    Temp.m_aaElement[ 1 ][ 2 ] *= S;
	    Temp.m_aaElement[ 2 ][ 2 ] *= S;
	    Temp.m_aaElement[ 3 ][ 2 ] *= S;

	    Temp.m_aaElement[ 0 ][ 3 ] *= S;
	    Temp.m_aaElement[ 1 ][ 3 ] *= S;
	    Temp.m_aaElement[ 2 ][ 3 ] *= S;
	    Temp.m_aaElement[ 3 ][ 3 ] *= S;
	    return ( Temp );
	}

	/**
	 * Matrix同士の比較<br>
	 * 主にCqRenderのmemcmpの代わりに使用する。<br>
	 * 全ての要素が等しければtrue<br>
	 * それ以外ではfalseを返す。<br>
	 * 
	 * @param mtx　比較するMatrix
	 * @return　true:全ての要素が等しい false:それ以外
	 */
	public boolean equals(CqMatrix mtx)
	{
		if(this.m_aaElement[0][0] != mtx.m_aaElement[0][0])return false;
		if(this.m_aaElement[0][1] != mtx.m_aaElement[0][1])return false;
		if(this.m_aaElement[0][2] != mtx.m_aaElement[0][2])return false;
		if(this.m_aaElement[0][3] != mtx.m_aaElement[0][3])return false;
		
		if(this.m_aaElement[1][0] != mtx.m_aaElement[1][0])return false;
		if(this.m_aaElement[1][1] != mtx.m_aaElement[1][1])return false;
		if(this.m_aaElement[1][2] != mtx.m_aaElement[1][2])return false;
		if(this.m_aaElement[1][3] != mtx.m_aaElement[1][3])return false;
		
		if(this.m_aaElement[2][0] != mtx.m_aaElement[2][0])return false;
		if(this.m_aaElement[2][1] != mtx.m_aaElement[2][1])return false;
		if(this.m_aaElement[2][2] != mtx.m_aaElement[2][2])return false;
		if(this.m_aaElement[2][3] != mtx.m_aaElement[2][3])return false;
		
		if(this.m_aaElement[3][0] != mtx.m_aaElement[3][0])return false;
		if(this.m_aaElement[3][1] != mtx.m_aaElement[3][1])return false;
		if(this.m_aaElement[3][2] != mtx.m_aaElement[3][2])return false;
		if(this.m_aaElement[3][3] != mtx.m_aaElement[3][3])return false;
		
		return true;
	}
	
// 2005.07.05 これらは行列式計算にのみ利用されていたので削除した。isawa
//
//	//************************************************************************//
//	//*** 広域関数としてのスタティックメソッド
//	//************************************************************************//
//	
	public static float det2x2( float a, float b, float c, float d )
	{
		return a * d - b * c;
	}
	
	public static float det3x3( float a1, float a2, float a3,
			float b1, float b2, float b3,
			float c1, float c2, float c3 )
	{
		return a1 * det2x2( b2, b3, c2, c3 ) -
		b1 * det2x2( a2, a3, c2, c3 ) +
		c1 * det2x2( a2, a3, b2, b3 );
	}
	

}
