Main Page | Namespace List | Class Hierarchy | Alphabetical List | Compound List | File List | Namespace Members | Compound Members | File Members

Matrix44.h

Go to the documentation of this file.
00001 //------------------------------------------------------------------------------
00002 // Lamp : Open source game middleware
00003 // Copyright (C) 2004  Junpei Ohtani ( Email : junpee@users.sourceforge.jp )
00004 //
00005 // This library is free software; you can redistribute it and/or
00006 // modify it under the terms of the GNU Lesser General Public
00007 // License as published by the Free Software Foundation; either
00008 // version 2.1 of the License, or (at your option) any later version.
00009 //
00010 // This library is distributed in the hope that it will be useful,
00011 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00012 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00013 // Lesser General Public License for more details.
00014 //
00015 // You should have received a copy of the GNU Lesser General Public
00016 // License along with this library; if not, write to the Free Software
00017 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00018 //------------------------------------------------------------------------------
00019 
00020 /** @file
00021  * 4×4行列ヘッダ
00022  * @author Junpee
00023  */
00024 
00025 #ifndef MATRIX44_H_
00026 #define MATRIX44_H_
00027 
00028 #include <Core/Primitive/Vector3.h>
00029 #include <Core/Primitive/Quaternion.h>
00030 
00031 namespace Lamp{
00032 
00033 class Matrix33;
00034 class Matrix34;
00035 
00036 //------------------------------------------------------------------------------
00037 /**
00038  * 4×4行列
00039  *
00040  * 右手座標系を使用します。
00041  * このクラスは継承しないで下さい。
00042  */
00043 class Matrix44{
00044 public:
00045     //--------------------------------------------------------------------------
00046     // メンバ変数
00047     //--------------------------------------------------------------------------
00048     /// メンバ変数
00049     union{
00050         /// 各要素
00051         struct{
00052             /// 00要素
00053             float m00;
00054             /// 01要素
00055             float m01;
00056             /// 02要素
00057             float m02;
00058             /// 03要素
00059             float m03;
00060             /// 10要素
00061             float m10;
00062             /// 11要素
00063             float m11;
00064             /// 12要素
00065             float m12;
00066             /// 13要素
00067             float m13;
00068             /// 20要素
00069             float m20;
00070             /// 21要素
00071             float m21;
00072             /// 22要素
00073             float m22;
00074             /// 23要素
00075             float m23;
00076             /// 30要素
00077             float m30;
00078             /// 31要素
00079             float m31;
00080             /// 32要素
00081             float m32;
00082             /// 33要素
00083             float m33;
00084         };
00085 
00086         /// 行列値
00087         float m[4][4];
00088 
00089         /// 配列
00090         float array[16];
00091     };
00092 
00093     //--------------------------------------------------------------------------
00094     // 定数
00095     //--------------------------------------------------------------------------
00096     /// ゼロ行列
00097     static const Matrix44 zero;
00098 
00099     /// 単位行列
00100     static const Matrix44 unit;
00101 
00102     //--------------------------------------------------------------------------
00103     // コンストラクタ
00104     //--------------------------------------------------------------------------
00105     /**
00106      * コンストラクタ
00107      *
00108      * このコンストラクタは初期値の設定を行わないため値は不定です。
00109      */
00110     Matrix44(){}
00111 
00112     /**
00113      * コンストラクタ
00114      * @param i00 m00の初期値
00115      * @param i01 m01の初期値
00116      * @param i02 m02の初期値
00117      * @param i03 m03の初期値
00118      * @param i10 m10の初期値
00119      * @param i11 m11の初期値
00120      * @param i12 m12の初期値
00121      * @param i13 m13の初期値
00122      * @param i20 m20の初期値
00123      * @param i21 m21の初期値
00124      * @param i22 m22の初期値
00125      * @param i23 m23の初期値
00126      * @param i30 m30の初期値
00127      * @param i31 m31の初期値
00128      * @param i32 m32の初期値
00129      * @param i33 m33の初期値
00130      */
00131     inline Matrix44(
00132         float i00, float i01, float i02, float i03,
00133         float i10, float i11, float i12, float i13,
00134         float i20, float i21, float i22, float i23,
00135         float i30, float i31, float i32, float i33) :
00136         m00(i00), m01(i01), m02(i02), m03(i03),
00137         m10(i10), m11(i11), m12(i12), m13(i13),
00138         m20(i20), m21(i21), m22(i22), m23(i23),
00139         m30(i30), m31(i31), m32(i32), m33(i33){
00140     }
00141 
00142     /**
00143      * コンストラクタ
00144      * @param source 初期値配列
00145      */
00146     inline explicit Matrix44(const float* const source) :
00147         m00(source[ 0]), m01(source[ 1]), m02(source[ 2]), m03(source[ 3]),
00148         m10(source[ 4]), m11(source[ 5]), m12(source[ 6]), m13(source[ 7]),
00149         m20(source[ 8]), m21(source[ 9]), m22(source[10]), m23(source[11]),
00150         m30(source[12]), m31(source[13]), m32(source[14]), m33(source[15]){
00151     }
00152 
00153     //--------------------------------------------------------------------------
00154     // 値の設定
00155     //--------------------------------------------------------------------------
00156     /**
00157      * 値の設定
00158      * @param s00 m00の設定値
00159      * @param s01 m01の設定値
00160      * @param s02 m02の設定値
00161      * @param s03 m03の設定値
00162      * @param s10 m10の設定値
00163      * @param s11 m11の設定値
00164      * @param s12 m12の設定値
00165      * @param s13 m13の設定値
00166      * @param s20 m20の設定値
00167      * @param s21 m21の設定値
00168      * @param s22 m22の設定値
00169      * @param s23 m23の設定値
00170      * @param s30 m30の設定値
00171      * @param s31 m31の設定値
00172      * @param s32 m32の設定値
00173      * @param s33 m33の設定値
00174      */
00175     inline void set(
00176         float s00, float s01, float s02, float s03,
00177         float s10, float s11, float s12, float s13,
00178         float s20, float s21, float s22, float s23,
00179         float s30, float s31, float s32, float s33){
00180         m00 = s00; m01 = s01; m02 = s02; m03 = s03;
00181         m10 = s10; m11 = s11; m12 = s12; m13 = s13;
00182         m20 = s20; m21 = s21; m22 = s22; m23 = s23;
00183         m30 = s30; m31 = s31; m32 = s32; m33 = s33;
00184     }
00185 
00186     /**
00187      * 値の設定
00188      * @param source 設定値配列
00189      */
00190     inline void set(const float* const source){
00191         m00 = source[ 0]; m01 = source[ 1]; m02 = source[ 2]; m03 = source[ 3];
00192         m10 = source[ 4]; m11 = source[ 5]; m12 = source[ 6]; m13 = source[ 7];
00193         m20 = source[ 8]; m21 = source[ 9]; m22 = source[10]; m23 = source[11];
00194         m30 = source[12]; m31 = source[13]; m32 = source[14]; m33 = source[15];
00195     }
00196 
00197     /**
00198      * ゼロ行列設定
00199      */
00200     inline void setZero(){
00201         set(0.f, 0.f, 0.f, 0.f,
00202             0.f, 0.f, 0.f, 0.f,
00203             0.f, 0.f, 0.f, 0.f,
00204             0.f, 0.f, 0.f, 0.f);
00205     }
00206 
00207     /**
00208      * 単位行列設定
00209      */
00210     inline void setUnit(){
00211         set(1.f, 0.f, 0.f, 0.f,
00212             0.f, 1.f, 0.f, 0.f,
00213             0.f, 0.f, 1.f, 0.f,
00214             0.f, 0.f, 0.f, 1.f);
00215     }
00216 
00217     /**
00218      * 3×3行列の設定
00219      * @param source 設定元行列
00220      */
00221     void set(const Matrix33& source);
00222 
00223     /**
00224      * 3×4行列の設定
00225      * @param source 設定元行列
00226      */
00227     void set(const Matrix34& source);
00228 
00229     //--------------------------------------------------------------------------
00230     // スケール
00231     //--------------------------------------------------------------------------
00232     /**
00233      * スケールの設定
00234      * @param scaleX Xスケール値
00235      * @param scaleY Yスケール値
00236      * @param scaleZ Zスケール値
00237      */
00238     inline void setScale(float scaleX, float scaleY, float scaleZ){
00239         set(scaleX,    0.f,    0.f,    0.f,
00240                0.f, scaleY,    0.f,    0.f,
00241                0.f,    0.f, scaleZ,    0.f,
00242                0.f,    0.f,    0.f,    1.f);
00243     }
00244 
00245     /**
00246      * スケールの設定
00247      * @param scale スケール値
00248      */
00249     inline void setScale(const Vector3& scale){
00250         setScale(scale.x, scale.y, scale.z);
00251     }
00252 
00253     /**
00254      * スケールの追加
00255      * @param scaleX Xスケール値
00256      * @param scaleY Yスケール値
00257      * @param scaleZ Zスケール値
00258      */
00259     inline void addScale(float scaleX, float scaleY, float scaleZ){
00260         Matrix44 matrix;
00261         matrix.setScale(scaleX, scaleY, scaleZ);
00262         (*this) = matrix * (*this);
00263     }
00264 
00265     /**
00266      * スケールの追加
00267      * @param scale スケール値
00268      */
00269     inline void addScale(const Vector3& scale){
00270         Matrix44 matrix;
00271         matrix.setScale(scale.x, scale.y, scale.z);
00272         (*this) = matrix * (*this);
00273     }
00274 
00275     //--------------------------------------------------------------------------
00276     // 回転
00277     //--------------------------------------------------------------------------
00278     /**
00279      * X軸回転の設定
00280      * @param radian ラジアン単位での回転角度
00281      */
00282     inline void setRotationX(float radian){
00283         float sin = Math::sin(radian);
00284         float cos = Math::cos(radian);
00285         set( 1.f,  0.f,  0.f,  0.f,
00286              0.f,  cos, -sin,  0.f,
00287              0.f,  sin,  cos,  0.f,
00288              0.f,  0.f,  0.f,  1.f);
00289     }
00290 
00291     /**
00292      * X軸回転の追加
00293      * @param radian ラジアン単位での回転角度
00294      */
00295     inline void addRotationX(float radian){
00296         Matrix44 matrix;
00297         matrix.setRotationX(radian);
00298         (*this) = matrix * (*this);
00299     }
00300 
00301     //--------------------------------------------------------------------------
00302     /**
00303      * Y軸回転の設定
00304      * @param radian ラジアン単位での回転角度
00305      */
00306     inline void setRotationY(float radian){
00307         float sin = Math::sin(radian);
00308         float cos = Math::cos(radian);
00309         set( cos,  0.f,  sin,  0.f,
00310              0.f,  1.f,  0.f,  0.f,
00311             -sin,  0.f,  cos,  0.f,
00312              0.f,  0.f,  0.f,  1.f);
00313     }
00314 
00315     /**
00316      * Y軸回転の追加
00317      * @param radian ラジアン単位での回転角度
00318      */
00319     inline void addRotationY(float radian){
00320         Matrix44 matrix;
00321         matrix.setRotationY(radian);
00322         (*this) = matrix * (*this);
00323     }
00324 
00325     //--------------------------------------------------------------------------
00326     /**
00327      * Z軸回転の設定
00328      * @param radian ラジアン単位での回転角度
00329      */
00330     inline void setRotationZ(float radian){
00331         float sin = Math::sin(radian);
00332         float cos = Math::cos(radian);
00333         set( cos, -sin,  0.f,  0.f,
00334              sin,  cos,  0.f,  0.f,
00335              0.f,  0.f,  1.f,  0.f,
00336              0.f,  0.f,  0.f,  1.f);
00337     }
00338 
00339     /**
00340      * Z軸回転の追加
00341      * @param radian ラジアン単位での回転角度
00342      */
00343     inline void addRotationZ(float radian){
00344         Matrix44 matrix;
00345         matrix.setRotationZ(radian);
00346         (*this) = matrix * (*this);
00347     }
00348 
00349     //--------------------------------------------------------------------------
00350     /**
00351      * 軸指定回転の設定
00352      * @param axis 回転軸
00353      * @param radian ラジアン単位での回転角度
00354      */
00355     inline void setRotationAxis(const Vector3& axis, float radian){
00356         Assert(axis.isUnit());
00357         float sin = Math::sin(radian);
00358         float cos = Math::cos(radian);
00359         float invCos = 1.f - cos;
00360         float xyInv = axis.x * axis.y * invCos;
00361         float xzInv = axis.x * axis.z * invCos;
00362         float yzInv = axis.y * axis.z * invCos;
00363         float xSin = axis.x * sin;
00364         float ySin = axis.y * sin;
00365         float zSin = axis.z * sin;
00366         set((axis.x * axis.x * invCos + cos), (xyInv - zSin), (xzInv + ySin),
00367             0.f,
00368             (xyInv + zSin), (axis.y * axis.y * invCos + cos), (yzInv - xSin),
00369             0.f,
00370             (xzInv - ySin), (yzInv + xSin), (axis.z * axis.z * invCos + cos),
00371             0.f,
00372             0.f, 0.f, 0.f, 1.f);
00373     }
00374 
00375     /**
00376      * 軸指定回転の追加
00377      * @param axis 回転軸
00378      * @param radian ラジアン単位での回転角度
00379      */
00380     inline void addRotationAxis(const Vector3& axis, float radian){
00381         Matrix44 matrix;
00382         matrix.setRotationAxis(axis, radian);
00383         (*this) = matrix * (*this);
00384     }
00385 
00386     /**
00387      * 軸指定回転の取得
00388      * @param axis [out]回転軸を格納するベクトルへのポインタ
00389      * @param radian [out]ラジアン単位での回転角度を格納するfloatへのポインタ
00390      */
00391     inline void getRotationAxis(Vector3* axis, float* radian) const{
00392         float radianResult = Math::acos(0.5f * ((m00 + m11 + m22) - 1.f));
00393         *radian = radianResult;
00394         if(radianResult > 0.f){
00395             if(radianResult < Math::PI){
00396                 axis->set(m21 - m12, m02 - m20, m10 - m01);
00397                 axis->normalize();
00398             }else{
00399                 if(m00 >= m11){
00400                     if(m00 >= m22){
00401                         axis->x = 0.5f * Math::sqrt(m00 - m11 - m22 + 1.f);
00402                         float halfInverse = 0.5f / axis->x;
00403                         axis->y = halfInverse * m01;
00404                         axis->z = halfInverse * m02;
00405                     }else{
00406                         axis->z = 0.5f * Math::sqrt(m22 - m00 - m11 + 1.f);
00407                         float halfInverse = 0.5f / axis->z;
00408                         axis->x = halfInverse * m02;
00409                         axis->y = halfInverse * m12;
00410                     }
00411                 }else{
00412                     if(m11 >= m22){
00413                         axis->y = 0.5f * Math::sqrt(m11 - m00 - m22 + 1.f);
00414                         float halfInverse = 0.5f / axis->y;
00415                         axis->x = halfInverse * m01;
00416                         axis->z = halfInverse * m12;
00417                     }else{
00418                         axis->z = 0.5f * Math::sqrt(m22 - m00 - m11 + 1.f);
00419                         float halfInverse = 0.5f / axis->z;
00420                         axis->x = halfInverse * m02;
00421                         axis->y = halfInverse * m12;
00422                     }
00423                 }
00424             }
00425         }else{
00426             axis->set(1.f, 0.f, 0.f);
00427         }
00428     }
00429 
00430     //--------------------------------------------------------------------------
00431     /**
00432      * 四元数回転の設定
00433      * @param quaternion 四元数
00434      */
00435     inline void setRotationQuaternion(const Quaternion& quaternion){
00436         Assert(quaternion.isUnit());
00437         float x2 = quaternion.x + quaternion.x;
00438         float y2 = quaternion.y + quaternion.y;
00439         float z2 = quaternion.z + quaternion.z;
00440         float xx2 = quaternion.x * x2;
00441         float xy2 = quaternion.x * y2;
00442         float xz2 = quaternion.x * z2;
00443         float yy2 = quaternion.y * y2;
00444         float yz2 = quaternion.y * z2;
00445         float zz2 = quaternion.z * z2;
00446         float wx2 = quaternion.w * x2;
00447         float wy2 = quaternion.w * y2;
00448         float wz2 = quaternion.w * z2;
00449         m00 = 1.f - (yy2 + zz2);
00450         m01 = xy2 - wz2;
00451         m02 = xz2 + wy2;
00452         m03 = 0.f;
00453 
00454         m10 = xy2 + wz2;
00455         m11 = 1.f - (xx2 + zz2);
00456         m12 = yz2 - wx2;
00457         m13 = 0.f;
00458 
00459         m20 = xz2 - wy2;
00460         m21 = yz2 + wx2;
00461         m22 = 1.f - (xx2 + yy2);
00462         m23 = 0.f;
00463 
00464         m30 = 0.f;
00465         m31 = 0.f;
00466         m32 = 0.f;
00467         m33 = 1.f;
00468     }
00469 
00470     /**
00471      * 四元数回転の追加
00472      * @param quaternion 四元数
00473      */
00474     inline void addRotationQuaternion(const Quaternion& quaternion){
00475         Matrix44 matrix;
00476         matrix.setRotationQuaternion(quaternion);
00477         (*this) = matrix * (*this);
00478     }
00479 
00480     /**
00481      * 四元数回転の取得
00482      * @return 回転を表す四元数
00483      */
00484     inline Quaternion getRotationQuaternion() const{
00485         Quaternion result;
00486         float trace = m00 + m11 + m22;
00487         if(trace > 0.f){
00488             float scale = Math::sqrt(trace + 1.f);
00489             result.w = scale * 0.5f;
00490             scale = 0.5f / scale;
00491             result.x = (m21 - m12) * scale;
00492             result.y = (m02 - m20) * scale;
00493             result.z = (m10 - m01) * scale;
00494         }else{
00495             int i = 0;
00496             if(m11 > m00){ i = 1; }
00497             if(m22 > m[i][i]){ i = 2; }
00498             int nextTable[] = { 1, 2, 0 };
00499             int j = nextTable[i];
00500             int k = nextTable[j];
00501             float scale = Math::sqrt(m[i][i] - m[j][j] - m[k][k] + 1.f);
00502             result.array[i] = 0.5f * scale;
00503             scale = 0.5f / scale;
00504             result.w = (m[k][j] - m[j][k]) * scale;
00505             result.array[j] = (m[j][i] + m[i][j]) * scale;
00506             result.array[k] = (m[k][i] + m[i][k]) * scale;
00507         }
00508         return result;
00509     }
00510 
00511     //--------------------------------------------------------------------------
00512     // 三軸指定回転
00513     //--------------------------------------------------------------------------
00514     /**
00515      * XYZ軸回転の設定
00516      * @param radian 各軸におけるラジアン単位での回転角度
00517      */
00518     inline void setRotationXYZ(const Vector3& radian){
00519         float sinX = Math::sin(radian.x);
00520         float cosX = Math::cos(radian.x);
00521         float sinY = Math::sin(radian.y);
00522         float cosY = Math::cos(radian.y);
00523         float sinZ = Math::sin(radian.z);
00524         float cosZ = Math::cos(radian.z);
00525         m00 = cosY * cosZ;
00526         m01 = sinX * sinY * cosZ - cosX * sinZ;
00527         m02 = cosX * sinY * cosZ + sinX * sinZ;
00528         m03 = 0.f;
00529 
00530         m10 = cosY * sinZ;
00531         m11 = sinX * sinY * sinZ + cosX * cosZ;
00532         m12 = cosX * sinY * sinZ - sinX * cosZ;
00533         m13 = 0.f;
00534 
00535         m20 = -sinY;
00536         m21 = sinX * cosY;
00537         m22 = cosX * cosY;
00538         m23 = 0.f;
00539 
00540         m30 = 0.f;
00541         m31 = 0.f;
00542         m32 = 0.f;
00543         m33 = 1.f;
00544     }
00545 
00546     /**
00547      * XYZ軸回転の追加
00548      * @param radian 各軸におけるラジアン単位での回転角度
00549      */
00550     inline void addRotationXYZ(const Vector3& radian){
00551         Matrix44 matrix;
00552         matrix.setRotationXYZ(radian);
00553         (*this) = matrix * (*this); 
00554     }
00555 
00556     /**
00557      * XYZ軸回転の取得
00558      * @param radian [out] 各軸におけるラジアン単位での回転角度
00559      * @return 答えが単一であればtrue
00560      */
00561     inline bool getRotationXYZ(Vector3* radian) const{
00562         float yRadian = Math::asin(-m20);
00563         radian->y = yRadian;
00564         if(yRadian < Math::halfPI){
00565             if(yRadian > -Math::halfPI){
00566                 radian->x = Math::atan2(m21, m22);
00567                 radian->z = Math::atan2(m10, m00);
00568                 return true;
00569             }else{
00570                 radian->x = -Math::atan2(m01, m11);
00571                 radian->z = 0.f;
00572                 return false;
00573             }
00574         }else{
00575             radian->x = Math::atan2(m01, m11);
00576             radian->z = 0.f;
00577             return false;
00578         }
00579     }
00580 
00581     //--------------------------------------------------------------------------
00582     /**
00583      * XZY軸回転の設定
00584      * @param radian 各軸におけるラジアン単位での回転角度
00585      */
00586     // 展開すれば高速化できる
00587     inline void setRotationXZY(const Vector3& radian){
00588         setRotationX(radian.x);
00589         addRotationZ(radian.z);
00590         addRotationY(radian.y);
00591     }
00592 
00593     /**
00594      * XZY軸回転の追加
00595      * @param radian 各軸におけるラジアン単位での回転角度
00596      */
00597     inline void addRotationXZY(const Vector3& radian){
00598         Matrix44 matrix;
00599         matrix.setRotationXZY(radian);
00600         (*this) = matrix * (*this); 
00601     }
00602 
00603     /**
00604      * XZY軸回転の取得
00605      * @param radian [out] 各軸におけるラジアン単位での回転角度
00606      * @return 答えが単一であればtrue
00607      */
00608     inline bool getRotationXZY(Vector3* radian) const{
00609         float zRadian = Math::asin(m10);
00610         radian->z = zRadian;
00611         if(zRadian < Math::halfPI){
00612             if(zRadian > -Math::halfPI){
00613                 radian->x = Math::atan2(-m12, m11);
00614                 radian->y = Math::atan2(-m20, m00);
00615                 return true;
00616             }else{
00617                 radian->x = -Math::atan2(m02, m22);
00618                 radian->y = 0.f;
00619                 return false;
00620             }
00621         }else{
00622             radian->x = Math::atan2(m02, m22);
00623             radian->y = 0.f;
00624             return false;
00625         }
00626     }
00627 
00628     //--------------------------------------------------------------------------
00629     /**
00630      * YXZ軸回転の設定
00631      * @param radian 各軸におけるラジアン単位での回転角度
00632      */
00633     // 展開すれば高速化できる
00634     inline void setRotationYXZ(const Vector3& radian){
00635         setRotationY(radian.y);
00636         addRotationX(radian.x);
00637         addRotationZ(radian.z);
00638     }
00639 
00640     /**
00641      * YXZ軸回転の追加
00642      * @param radian 各軸におけるラジアン単位での回転角度
00643      */
00644     inline void addRotationYXZ(const Vector3& radian){
00645         Matrix44 matrix;
00646         matrix.setRotationYXZ(radian);
00647         (*this) = matrix * (*this); 
00648     }
00649 
00650     /**
00651      * YXZ軸回転の取得
00652      * @param radian [out] 各軸におけるラジアン単位での回転角度
00653      * @return 答えが単一であればtrue
00654      */
00655     inline bool getRotationYXZ(Vector3* radian) const{
00656         float xRadian = Math::asin(m21);
00657         radian->x = xRadian;
00658         if(xRadian < Math::halfPI){
00659             if(xRadian > -Math::halfPI){
00660                 radian->y = Math::atan2(-m20, m22);
00661                 radian->z = Math::atan2(-m01, m11);
00662                 return true;
00663             }else{
00664                 radian->y = -Math::atan2(-m10, m00);
00665                 radian->z = 0.f;
00666                 return false;
00667             }
00668         }else{
00669             radian->y = Math::atan2(-m10, m00);
00670             radian->z = 0.f;
00671             return false;
00672         }
00673     }
00674 
00675     //--------------------------------------------------------------------------
00676     /**
00677      * YZX軸回転の設定
00678      * @param radian 各軸におけるラジアン単位での回転角度
00679      */
00680     // 展開すれば高速化できる
00681     inline void setRotationYZX(const Vector3& radian){
00682         setRotationY(radian.y);
00683         addRotationZ(radian.z);
00684         addRotationX(radian.x);
00685     }
00686 
00687     /**
00688      * YZX軸回転の追加
00689      * @param radian 各軸におけるラジアン単位での回転角度
00690      */
00691     inline void addRotationYZX(const Vector3& radian){
00692         Matrix44 matrix;
00693         matrix.setRotationYZX(radian);
00694         (*this) = matrix * (*this); 
00695     }
00696 
00697     /**
00698      * YZX軸回転の取得
00699      * @param radian [out] 各軸におけるラジアン単位での回転角度
00700      * @return 答えが単一であればtrue
00701      */
00702     inline bool getRotationYZX(Vector3* radian) const{
00703         float zRadian = Math::asin(-m01);
00704         radian->z = zRadian;
00705         if(zRadian < Math::halfPI){
00706             if(zRadian > -Math::halfPI){
00707                 radian->y = Math::atan2(m02, m00);
00708                 radian->x = Math::atan2(m21, m11);
00709                 return true;
00710             }else{
00711                 radian->y = -Math::atan2(m12, m22);
00712                 radian->x = 0.f;
00713                 return false;
00714             }
00715         }else{
00716             radian->y = Math::atan2(m12, m22);
00717             radian->x = 0.f;
00718             return false;
00719         }
00720     }
00721 
00722     //--------------------------------------------------------------------------
00723     /**
00724      * ZXY軸回転の設定
00725      * @param radian 各軸におけるラジアン単位での回転角度
00726      */
00727     // 展開すれば高速化できる
00728     inline void setRotationZXY(const Vector3& radian){
00729         setRotationZ(radian.z);
00730         addRotationX(radian.x);
00731         addRotationY(radian.y);
00732     }
00733 
00734     /**
00735      * ZXY軸回転の追加
00736      * @param radian 各軸におけるラジアン単位での回転角度
00737      */
00738     inline void addRotationZXY(const Vector3& radian){
00739         Matrix44 matrix;
00740         matrix.setRotationZXY(radian);
00741         (*this) = matrix * (*this); 
00742     }
00743 
00744     /**
00745      * ZXY軸回転の取得
00746      * @param radian [out] 各軸におけるラジアン単位での回転角度
00747      * @return 答えが単一であればtrue
00748      */
00749     inline bool getRotationZXY(Vector3* radian) const{
00750         float xRadian = Math::asin(-m12);
00751         radian->x = xRadian;
00752         if(xRadian < Math::halfPI){
00753             if(xRadian > -Math::halfPI){
00754                 radian->z = Math::atan2(m10, m11);
00755                 radian->y = Math::atan2(m02, m22);
00756                 return true;
00757             }else{
00758                 radian->z = -Math::atan2(m20, m00);
00759                 radian->y = 0.f;
00760                 return false;
00761             }
00762         }else{
00763             radian->z = Math::atan2(m20, m00);
00764             radian->y = 0.f;
00765             return false;
00766         }
00767     }
00768 
00769     //--------------------------------------------------------------------------
00770     /**
00771      * ZYX軸回転の設定
00772      * @param radian 各軸におけるラジアン単位での回転角度
00773      */
00774     // 展開すれば高速化できる
00775     inline void setRotationZYX(const Vector3& radian){
00776         setRotationZ(radian.z);
00777         addRotationY(radian.y);
00778         addRotationX(radian.x);
00779     }
00780 
00781     /**
00782      * ZYX軸回転の追加
00783      * @param radian 各軸におけるラジアン単位での回転角度
00784      */
00785     inline void addRotationZYX(const Vector3& radian){
00786         Matrix44 matrix;
00787         matrix.setRotationZYX(radian);
00788         (*this) = matrix * (*this); 
00789     }
00790 
00791     /**
00792      * ZYX軸回転の取得
00793      * @param radian [out] 各軸におけるラジアン単位での回転角度
00794      * @return 答えが単一であればtrue
00795      */
00796     inline bool getRotationZYX(Vector3* radian) const{
00797         float yRadian = Math::asin(m02);
00798         radian->y = yRadian;
00799         if(yRadian < Math::halfPI){
00800             if(yRadian > -Math::halfPI){
00801                 radian->z = Math::atan2(-m01, m00);
00802                 radian->x = Math::atan2(-m12, m22);
00803                 return true;
00804             }else{
00805                 radian->z = -Math::atan2(-m10, m20);
00806                 radian->x = 0.f;
00807                 return false;
00808             }
00809         }else{
00810             radian->z = Math::atan2(m10, -m20);
00811             radian->x = 0.f;
00812             return false;
00813         }
00814     }
00815 
00816     //--------------------------------------------------------------------------
00817     // 移動
00818     //--------------------------------------------------------------------------
00819     /**
00820      * 移動の設定
00821      * @param translationX X移動値
00822      * @param translationY Y移動値
00823      * @param translationZ Z移動値
00824      */
00825     inline void setTranslation(
00826         float translationX, float translationY, float translationZ){
00827         set(1.f, 0.f, 0.f, translationX,
00828             0.f, 1.f, 0.f, translationY,
00829             0.f, 0.f, 1.f, translationZ,
00830             0.f, 0.f, 0.f, 1.f);
00831     }
00832 
00833     /**
00834      * 移動の設定
00835      * @param translation 移動値
00836      */
00837     inline void setTranslation(const Vector3& translation){
00838         setTranslation(translation.x, translation.y, translation.z);
00839     }
00840 
00841     /**
00842      * 移動の追加
00843      * @param translationX X移動値
00844      * @param translationY Y移動値
00845      * @param translationZ Z移動値
00846      */
00847     inline void addTranslation(
00848         float translationX, float translationY, float translationZ){
00849         Matrix44 matrix;
00850         matrix.setTranslation(translationX, translationY, translationZ);
00851         (*this) = matrix * (*this);
00852     }
00853 
00854     /**
00855      * 移動の追加
00856      * @param translation 移動値
00857      */
00858     inline void addTranslation(const Vector3& translation){
00859         Matrix44 matrix;
00860         matrix.setTranslation(translation.x, translation.y, translation.z);
00861         (*this) = matrix * (*this);
00862     }
00863 
00864     /**
00865      * 移動の取得
00866      * @return 移動値
00867      */
00868     inline Vector3 getTranslation() const{ return Vector3(m03, m13, m23); }
00869 
00870     //--------------------------------------------------------------------------
00871     // 変換
00872     //--------------------------------------------------------------------------
00873     /**
00874      * XYZ回転の変換設定
00875      *
00876      * XYZ回転、移動の順に変換する行列を作成します
00877      * @param radian 各軸におけるラジアン単位での回転角度
00878      * @param translation 移動値
00879      */
00880     inline void setTransformationXYZ(
00881         const Vector3& radian, const Vector3& translation){
00882         float sinX = Math::sin(radian.x);
00883         float cosX = Math::cos(radian.x);
00884         float sinY = Math::sin(radian.y);
00885         float cosY = Math::cos(radian.y);
00886         float sinZ = Math::sin(radian.z);
00887         float cosZ = Math::cos(radian.z);
00888         m00 = cosY * cosZ;
00889         m01 = sinX * sinY * cosZ - cosX * sinZ;
00890         m02 = cosX * sinY * cosZ + sinX * sinZ;
00891         m03 = translation.x;
00892 
00893         m10 = cosY * sinZ;
00894         m11 = sinX * sinY * sinZ + cosX * cosZ;
00895         m12 = cosX * sinY * sinZ - sinX * cosZ;
00896         m13 = translation.y;
00897 
00898         m20 = -sinY;
00899         m21 = sinX * cosY;
00900         m22 = cosX * cosY;
00901         m23 = translation.z;
00902 
00903         m30 = 0.f;
00904         m31 = 0.f;
00905         m32 = 0.f;
00906         m33 = 1.f;
00907     }
00908 
00909     /**
00910      * XYZ回転の変換追加
00911      *
00912      * XYZ回転、移動の順に変換を追加します
00913      * @param radian 各軸におけるラジアン単位での回転角度
00914      * @param translation 移動値
00915      */
00916     inline void addTransformationXYZ(
00917         const Vector3& radian, const Vector3& translation){
00918         Matrix44 matrix;
00919         matrix.setTransformationXYZ(radian, translation);
00920         (*this) = matrix * (*this); 
00921     }
00922 
00923     //--------------------------------------------------------------------------
00924     /**
00925      * XYZ回転の変換設定
00926      *
00927      * スケール、XYZ回転、移動の順に変換する行列を作成します
00928      * @param scale スケール値
00929      * @param radian 各軸におけるラジアン単位での回転角度
00930      * @param translation 移動値
00931      */
00932     inline void setTransformationXYZ(const Vector3& scale,
00933         const Vector3& radian, const Vector3& translation){
00934         float sinX = Math::sin(radian.x);
00935         float cosX = Math::cos(radian.x);
00936         float sinY = Math::sin(radian.y);
00937         float cosY = Math::cos(radian.y);
00938         float sinZ = Math::sin(radian.z);
00939         float cosZ = Math::cos(radian.z);
00940         m00 = scale.x * (cosY * cosZ);
00941         m01 = scale.y * (sinX * sinY * cosZ - cosX * sinZ);
00942         m02 = scale.z * (cosX * sinY * cosZ + sinX * sinZ);
00943         m03 = translation.x;
00944 
00945         m10 = scale.x * (cosY * sinZ);
00946         m11 = scale.y * (sinX * sinY * sinZ + cosX * cosZ);
00947         m12 = scale.z * (cosX * sinY * sinZ - sinX * cosZ);
00948         m13 = translation.y;
00949 
00950         m20 = scale.x * (-sinY);
00951         m21 = scale.y * (sinX * cosY);
00952         m22 = scale.z * (cosX * cosY);
00953         m23 = translation.z;
00954 
00955         m30 = 0.f;
00956         m31 = 0.f;
00957         m32 = 0.f;
00958         m33 = 1.f;
00959     }
00960 
00961     /**
00962      * XYZ回転の変換追加
00963      *
00964      * スケール、XYZ回転、移動の順に変換を追加します
00965      * @param scale スケール値
00966      * @param radian 各軸におけるラジアン単位での回転角度
00967      * @param translation 移動値
00968      */
00969     inline void addTransformationXYZ(const Vector3& scale,
00970         const Vector3& radian, const Vector3& translation){
00971         Matrix44 matrix;
00972         matrix.setTransformationXYZ(scale, radian, translation);
00973         (*this) = matrix * (*this); 
00974     }
00975 
00976     //--------------------------------------------------------------------------
00977     /**
00978      * 四元数回転の変換設定
00979      *
00980      * 四元数回転、移動の順に変換する行列を作成します
00981      * @param translation 移動値
00982      * @param quaternion 四元数
00983      */
00984     inline void setTransformationQuaternion(
00985         const Quaternion& quaternion, const Vector3& translation){
00986         Assert(quaternion.isUnit());
00987         float x2 = quaternion.x + quaternion.x;
00988         float y2 = quaternion.y + quaternion.y;
00989         float z2 = quaternion.z + quaternion.z;
00990         float xx2 = quaternion.x * x2;
00991         float xy2 = quaternion.x * y2;
00992         float xz2 = quaternion.x * z2;
00993         float yy2 = quaternion.y * y2;
00994         float yz2 = quaternion.y * z2;
00995         float zz2 = quaternion.z * z2;
00996         float wx2 = quaternion.w * x2;
00997         float wy2 = quaternion.w * y2;
00998         float wz2 = quaternion.w * z2;
00999         m00 = 1.f - (yy2 + zz2);
01000         m01 = xy2 - wz2;
01001         m02 = xz2 + wy2;
01002         m03 = translation.x;
01003 
01004         m10 = xy2 + wz2;
01005         m11 = 1.f - (xx2 + zz2);
01006         m12 = yz2 - wx2;
01007         m13 = translation.y;
01008 
01009         m20 = xz2 - wy2;
01010         m21 = yz2 + wx2;
01011         m22 = 1.f - (xx2 + yy2);
01012         m23 = translation.z;
01013 
01014         m30 = 0.f;
01015         m31 = 0.f;
01016         m32 = 0.f;
01017         m33 = 1.f;
01018     }
01019 
01020     /**
01021      * 四元数回転の変換追加
01022      *
01023      * 四元数回転、移動の順に変換を追加します
01024      * @param translation 移動値
01025      * @param quaternion 四元数
01026      */
01027     inline void addTransformationQuaternion(
01028         const Quaternion& quaternion, const Vector3& translation){
01029         Matrix44 matrix;
01030         matrix.setTransformationQuaternion(quaternion, translation);
01031         (*this) = matrix * (*this); 
01032     }
01033 
01034     //--------------------------------------------------------------------------
01035     /**
01036      * 四元数回転の変換設定
01037      *
01038      * スケール、四元数回転、移動の順に変換する行列を作成します
01039      * @param scale スケール値
01040      * @param translation 移動値
01041      * @param quaternion 四元数
01042      */
01043     inline void setTransformationQuaternion(const Vector3& scale,
01044         const Quaternion& quaternion, const Vector3& translation){
01045         Assert(quaternion.isUnit());
01046         float x2 = quaternion.x + quaternion.x;
01047         float y2 = quaternion.y + quaternion.y;
01048         float z2 = quaternion.z + quaternion.z;
01049         float xx2 = quaternion.x * x2;
01050         float xy2 = quaternion.x * y2;
01051         float xz2 = quaternion.x * z2;
01052         float yy2 = quaternion.y * y2;
01053         float yz2 = quaternion.y * z2;
01054         float zz2 = quaternion.z * z2;
01055         float wx2 = quaternion.w * x2;
01056         float wy2 = quaternion.w * y2;
01057         float wz2 = quaternion.w * z2;
01058         m00 = scale.x * (1.f - (yy2 + zz2));
01059         m01 = scale.y * (xy2 - wz2);
01060         m02 = scale.z * (xz2 + wy2);
01061         m03 = translation.x;
01062 
01063         m10 = scale.x * (xy2 + wz2);
01064         m11 = scale.y * (1.f - (xx2 + zz2));
01065         m12 = scale.z * (yz2 - wx2);
01066         m13 = translation.y;
01067 
01068         m20 = scale.x * (xz2 - wy2);
01069         m21 = scale.y * (yz2 + wx2);
01070         m22 = scale.z * (1.f - (xx2 + yy2));
01071         m23 = translation.z;
01072 
01073         m30 = 0.f;
01074         m31 = 0.f;
01075         m32 = 0.f;
01076         m33 = 1.f;
01077     }
01078 
01079     /**
01080      * 四元数回転の変換追加
01081      *
01082      * スケール、四元数回転、移動の順に変換を追加します
01083      * @param scale スケール値
01084      * @param translation 移動値
01085      * @param quaternion 四元数
01086      */
01087     inline void addTransformationQuaternion(const Vector3& scale,
01088         const Quaternion& quaternion, const Vector3& translation){
01089         Matrix44 matrix;
01090         matrix.setTransformationQuaternion(scale, quaternion, translation);
01091         (*this) = matrix * (*this); 
01092     }
01093 
01094     //--------------------------------------------------------------------------
01095     // 乗算
01096     //--------------------------------------------------------------------------
01097     /**
01098      * 行列乗算
01099      *
01100      * 左側の行列から順に計算が適用されます。
01101      * @param mtx 乗算する行列
01102      * @return 乗算された行列
01103      */
01104     inline Matrix44 operator *(const Matrix44& mtx) const{
01105         return Matrix44(
01106             (m00 * mtx.m00) + (m01 * mtx.m10) + (m02 * mtx.m20) + (m03 * mtx.m30),
01107             (m00 * mtx.m01) + (m01 * mtx.m11) + (m02 * mtx.m21) + (m03 * mtx.m31),
01108             (m00 * mtx.m02) + (m01 * mtx.m12) + (m02 * mtx.m22) + (m03 * mtx.m32),
01109             (m00 * mtx.m03) + (m01 * mtx.m13) + (m02 * mtx.m23) + (m03 * mtx.m33),
01110             (m10 * mtx.m00) + (m11 * mtx.m10) + (m12 * mtx.m20) + (m13 * mtx.m30),
01111             (m10 * mtx.m01) + (m11 * mtx.m11) + (m12 * mtx.m21) + (m13 * mtx.m31),
01112             (m10 * mtx.m02) + (m11 * mtx.m12) + (m12 * mtx.m22) + (m13 * mtx.m32),
01113             (m10 * mtx.m03) + (m11 * mtx.m13) + (m12 * mtx.m23) + (m13 * mtx.m33),
01114             (m20 * mtx.m00) + (m21 * mtx.m10) + (m22 * mtx.m20) + (m23 * mtx.m30),
01115             (m20 * mtx.m01) + (m21 * mtx.m11) + (m22 * mtx.m21) + (m23 * mtx.m31),
01116             (m20 * mtx.m02) + (m21 * mtx.m12) + (m22 * mtx.m22) + (m23 * mtx.m32),
01117             (m20 * mtx.m03) + (m21 * mtx.m13) + (m22 * mtx.m23) + (m23 * mtx.m33),
01118             (m30 * mtx.m00) + (m31 * mtx.m10) + (m32 * mtx.m20) + (m33 * mtx.m30),
01119             (m30 * mtx.m01) + (m31 * mtx.m11) + (m32 * mtx.m21) + (m33 * mtx.m31),
01120             (m30 * mtx.m02) + (m31 * mtx.m12) + (m32 * mtx.m22) + (m33 * mtx.m32),
01121             (m30 * mtx.m03) + (m31 * mtx.m13) + (m32 * mtx.m23) + (m33 * mtx.m33));
01122     }
01123 
01124     /**
01125      * 代入行列乗算
01126      *
01127      * 左側の行列から順に計算が適用されます。
01128      * @param mtx 乗算する行列
01129      * @return 乗算された行列
01130      */
01131     inline Matrix44& operator *=(Matrix44 mtx){
01132         float old00 = m00;
01133         float old01 = m01;
01134         float old02 = m02;
01135         m00 = (old00 * mtx.m00) + (old01 * mtx.m10) +
01136             (old02 * mtx.m20) + (m03 * mtx.m30);
01137         m01 = (old00 * mtx.m01) + (old01 * mtx.m11) +
01138             (old02 * mtx.m21) + (m03 * mtx.m31);
01139         m02 = (old00 * mtx.m02) + (old01 * mtx.m12) +
01140             (old02 * mtx.m22) + (m03 * mtx.m32);
01141         m03 = (old00 * mtx.m03) + (old01 * mtx.m13) +
01142             (old02 * mtx.m23) + (m03 * mtx.m33);
01143         float old10 = m10;
01144         float old11 = m11;
01145         float old12 = m12;
01146         m10 = (old10 * mtx.m00) + (old11 * mtx.m10) +
01147             (old12 * mtx.m20) + (m13 * mtx.m30);
01148         m11 = (old10 * mtx.m01) + (old11 * mtx.m11) +
01149             (old12 * mtx.m21) + (m13 * mtx.m31);
01150         m12 = (old10 * mtx.m02) + (old11 * mtx.m12) +
01151             (old12 * mtx.m22) + (m13 * mtx.m32);
01152         m13 = (old10 * mtx.m03) + (old11 * mtx.m13) +
01153             (old12 * mtx.m23) + (m13 * mtx.m33);
01154         float old20 = m20;
01155         float old21 = m21;
01156         float old22 = m22;
01157         m20 = (old20 * mtx.m00) + (old21 * mtx.m10) +
01158             (old22 * mtx.m20) + (m23 * mtx.m30);
01159         m21 = (old20 * mtx.m01) + (old21 * mtx.m11) +
01160             (old22 * mtx.m21) + (m23 * mtx.m31);
01161         m22 = (old20 * mtx.m02) + (old21 * mtx.m12) +
01162             (old22 * mtx.m22) + (m23 * mtx.m32);
01163         m23 = (old20 * mtx.m03) + (old21 * mtx.m13) +
01164             (old22 * mtx.m23) + (m23 * mtx.m33);
01165         float old30 = m30;
01166         float old31 = m31;
01167         float old32 = m32;
01168         m30 = (old30 * mtx.m00) + (old31 * mtx.m10) +
01169             (old32 * mtx.m20) + (m33 * mtx.m30);
01170         m31 = (old30 * mtx.m01) + (old31 * mtx.m11) +
01171             (old32 * mtx.m21) + (m33 * mtx.m31);
01172         m32 = (old30 * mtx.m02) + (old31 * mtx.m12) +
01173             (old32 * mtx.m22) + (m33 * mtx.m32);
01174         m33 = (old30 * mtx.m03) + (old31 * mtx.m13) +
01175             (old32 * mtx.m23) + (m33 * mtx.m33);
01176         return *this;
01177     }
01178 
01179     /**
01180      * ベクトル乗算
01181      * @param vector 乗算するベクトル
01182      * @return 乗算されたベクトル
01183      */
01184     // W要素を考慮していない。Vector4との乗算か、W = 1で算出するか。
01185     inline Vector3 operator *(const Vector3& vector) const{
01186         return Vector3(
01187             vector.x * m00 + vector.y * m01 + vector.z * m02 + m03,
01188             vector.x * m10 + vector.y * m11 + vector.z * m12 + m13,
01189             vector.x * m20 + vector.y * m21 + vector.z * m22 + m23);
01190     }
01191 
01192     /**
01193      * 3x3のベクトル乗算
01194      * @param vector 3x3の乗算をするベクトル
01195      * @return 3x3の乗算されたベクトル
01196      */
01197     inline Vector3 multiply33(const Vector3& vector) const{
01198         return Vector3(
01199             vector.x * m00 + vector.y * m01 + vector.z * m02,
01200             vector.x * m10 + vector.y * m11 + vector.z * m12,
01201             vector.x * m20 + vector.y * m21 + vector.z * m22);
01202     }
01203 
01204     /**
01205      * スカラー乗算
01206      * @param value 乗算するスカラー
01207      * @return 乗算された行列
01208      */
01209     inline Matrix44 operator *(float value) const{
01210         return Matrix44(
01211             m00 * value, m01 * value, m02 * value, m03 * value,
01212             m10 * value, m11 * value, m12 * value, m13 * value,
01213             m20 * value, m21 * value, m22 * value, m23 * value,
01214             m30 * value, m31 * value, m32 * value, m33 * value);
01215     }
01216 
01217     /**
01218      * スカラー乗算
01219      * @param value 乗算するスカラー
01220      * @return 乗算された行列
01221      */
01222     inline Matrix44& operator *=(float value){
01223         m00 *= value;
01224         m01 *= value;
01225         m02 *= value;
01226         m03 *= value;
01227         m10 *= value;
01228         m11 *= value;
01229         m12 *= value;
01230         m13 *= value;
01231         m20 *= value;
01232         m21 *= value;
01233         m22 *= value;
01234         m23 *= value;
01235         m30 *= value;
01236         m31 *= value;
01237         m32 *= value;
01238         m33 *= value;
01239         return *this;
01240     }
01241 
01242     //--------------------------------------------------------------------------
01243     // 行列演算
01244     //--------------------------------------------------------------------------
01245     /**
01246      * 転置
01247      */
01248     inline void transpose(){
01249         float swap;
01250         swap = m01; m01 = m10; m10 = swap;
01251         swap = m02; m02 = m20; m20 = swap;
01252         swap = m03; m03 = m30; m30 = swap;
01253         swap = m12; m12 = m21; m21 = swap;
01254         swap = m13; m13 = m31; m31 = swap;
01255         swap = m23; m23 = m32; m32 = swap;
01256     }
01257 
01258     /**
01259      * 行列式
01260      * @return 行列式
01261      */
01262     inline float determinant() const{
01263         return
01264             m00 * (
01265                 m11 * (m22 * m33 - m32 * m23) -
01266                 m12 * (m21 * m33 - m31 * m23) +
01267                 m13 * (m21 * m32 - m31 * m22)
01268             ) -
01269             m01 * (
01270                 m10 * (m22 * m33 - m32 * m23) -
01271                 m12 * (m20 * m33 - m30 * m23) +
01272                 m13 * (m20 * m32 - m30 * m22)
01273             ) +
01274             m02 * (
01275                 m10 * (m21 * m33 - m31 * m23) -
01276                 m11 * (m20 * m33 - m30 * m23) +
01277                 m13 * (m20 * m31 - m30 * m21)
01278             ) -
01279             m03 * (
01280                 m10 * (m21 * m32 - m31 * m22) -
01281                 m11 * (m20 * m32 - m30 * m22) +
01282                 m12 * (m20 * m31 - m30 * m21)
01283             );
01284     }
01285 
01286     /**
01287      * 逆行列
01288      * @return 行列式
01289      */
01290     inline float invert(){
01291         Matrix44 invertMatrix;
01292         // 行列式を出す
01293         invertMatrix.m00 = (
01294             m11 * (m22 * m33 - m32 * m23) -
01295             m12 * (m21 * m33 - m31 * m23) +
01296             m13 * (m21 * m32 - m31 * m22));
01297         invertMatrix.m10 = -(
01298             m10 * (m22 * m33 - m32 * m23) -
01299             m12 * (m20 * m33 - m30 * m23) +
01300             m13 * (m20 * m32 - m30 * m22));
01301         invertMatrix.m20 = (
01302             m10 * (m21 * m33 - m31 * m23) -
01303             m11 * (m20 * m33 - m30 * m23) +
01304             m13 * (m20 * m31 - m30 * m21));
01305         invertMatrix.m30 = -(
01306             m10 * (m21 * m32 - m31 * m22) -
01307             m11 * (m20 * m32 - m30 * m22) +
01308             m12 * (m20 * m31 - m30 * m21));
01309         float determ =
01310             m00 * invertMatrix.m00 +
01311             m01 * invertMatrix.m10 +
01312             m02 * invertMatrix.m20 +
01313             m03 * invertMatrix.m30;
01314         Assert(Math::abs(determ) > Math::epsilon);
01315         // 各要素の算出
01316         invertMatrix.m01 = -(
01317             m01 * (m22 * m33 - m32 * m23) -
01318             m02 * (m21 * m33 - m31 * m23) +
01319             m03 * (m21 * m32 - m31 * m22));
01320         invertMatrix.m02 = (
01321             m01 * (m12 * m33 - m32 * m13) -
01322             m02 * (m11 * m33 - m31 * m13) +
01323             m03 * (m11 * m32 - m31 * m12));
01324         invertMatrix.m03 = -(
01325             m01 * (m12 * m23 - m22 * m13) -
01326             m02 * (m11 * m23 - m21 * m13) +
01327             m03 * (m11 * m22 - m21 * m12));
01328         invertMatrix.m11 = (
01329             m00 * (m22 * m33 - m32 * m23) -
01330             m02 * (m20 * m33 - m30 * m23) +
01331             m03 * (m20 * m32 - m30 * m22));
01332         invertMatrix.m12 = -(
01333             m00 * (m12 * m33 - m32 * m13) -
01334             m02 * (m10 * m33 - m30 * m13) +
01335             m03 * (m10 * m32 - m30 * m12));
01336         invertMatrix.m13 = (
01337             m00 * (m12 * m23 - m22 * m13) -
01338             m02 * (m10 * m23 - m20 * m13) +
01339             m03 * (m10 * m22 - m20 * m12));
01340         invertMatrix.m21 = -(
01341             m00 * (m21 * m33 - m31 * m23) -
01342             m01 * (m20 * m33 - m30 * m23) +
01343             m03 * (m20 * m31 - m30 * m21));
01344         invertMatrix.m22 = (
01345             m00 * (m11 * m33 - m31 * m13) -
01346             m01 * (m10 * m33 - m30 * m13) +
01347             m03 * (m10 * m31 - m30 * m11));
01348         invertMatrix.m23 = -(
01349             m00 * (m11 * m23 - m21 * m13) -
01350             m01 * (m10 * m23 - m20 * m13) +
01351             m03 * (m10 * m21 - m20 * m11));
01352         invertMatrix.m31 = (
01353             m00 * (m21 * m32 - m31 * m22) -
01354             m01 * (m20 * m32 - m30 * m22) +
01355             m02 * (m20 * m31 - m30 * m21));
01356         invertMatrix.m32 = -(
01357             m00 * (m11 * m32 - m31 * m12) -
01358             m01 * (m10 * m32 - m30 * m12) +
01359             m02 * (m10 * m31 - m30 * m11));
01360         invertMatrix.m33 = (
01361             m00 * (m11 * m22 - m21 * m12) -
01362             m01 * (m10 * m22 - m20 * m12) +
01363             m02 * (m10 * m21 - m20 * m11));
01364         // 行列式の逆数を掛ける
01365         float invDeterm = 1.f / determ;
01366         invertMatrix *= invDeterm;
01367         (*this) = invertMatrix;
01368         return determ;
01369     }
01370 
01371     /**
01372      * 逆行列
01373      * @param invertMatrix [out] 逆行列を格納する行列へのポインタ
01374      * @return 行列式
01375      */
01376     inline float invert(Matrix44* invertMatrix) const{
01377         Assert(invertMatrix != NULL);
01378         Assert(invertMatrix != this);
01379         // 行列式を出す
01380         invertMatrix->m00 = (
01381             m11 * (m22 * m33 - m32 * m23) -
01382             m12 * (m21 * m33 - m31 * m23) +
01383             m13 * (m21 * m32 - m31 * m22));
01384         invertMatrix->m10 = -(
01385             m10 * (m22 * m33 - m32 * m23) -
01386             m12 * (m20 * m33 - m30 * m23) +
01387             m13 * (m20 * m32 - m30 * m22));
01388         invertMatrix->m20 = (
01389             m10 * (m21 * m33 - m31 * m23) -
01390             m11 * (m20 * m33 - m30 * m23) +
01391             m13 * (m20 * m31 - m30 * m21));
01392         invertMatrix->m30 = -(
01393             m10 * (m21 * m32 - m31 * m22) -
01394             m11 * (m20 * m32 - m30 * m22) +
01395             m12 * (m20 * m31 - m30 * m21));
01396         float determ =
01397             m00 * invertMatrix->m00 +
01398             m01 * invertMatrix->m10 +
01399             m02 * invertMatrix->m20 +
01400             m03 * invertMatrix->m30;
01401         Assert(determ > Math::epsilon);
01402         // 各要素の算出
01403         invertMatrix->m01 = -(
01404             m01 * (m22 * m33 - m32 * m23) -
01405             m02 * (m21 * m33 - m31 * m23) +
01406             m03 * (m21 * m32 - m31 * m22));
01407         invertMatrix->m02 = (
01408             m01 * (m12 * m33 - m32 * m13) -
01409             m02 * (m11 * m33 - m31 * m13) +
01410             m03 * (m11 * m32 - m31 * m12));
01411         invertMatrix->m03 = -(
01412             m01 * (m12 * m23 - m22 * m13) -
01413             m02 * (m11 * m23 - m21 * m13) +
01414             m03 * (m11 * m22 - m21 * m12));
01415         invertMatrix->m11 = (
01416             m00 * (m22 * m33 - m32 * m23) -
01417             m02 * (m20 * m33 - m30 * m23) +
01418             m03 * (m20 * m32 - m30 * m22));
01419         invertMatrix->m12 = -(
01420             m00 * (m12 * m33 - m32 * m13) -
01421             m02 * (m10 * m33 - m30 * m13) +
01422             m03 * (m10 * m32 - m30 * m12));
01423         invertMatrix->m13 = (
01424             m00 * (m12 * m23 - m22 * m13) -
01425             m02 * (m10 * m23 - m20 * m13) +
01426             m03 * (m10 * m22 - m20 * m12));
01427         invertMatrix->m21 = -(
01428             m00 * (m21 * m33 - m31 * m23) -
01429             m01 * (m20 * m33 - m30 * m23) +
01430             m03 * (m20 * m31 - m30 * m21));
01431         invertMatrix->m22 = (
01432             m00 * (m11 * m33 - m31 * m13) -
01433             m01 * (m10 * m33 - m30 * m13) +
01434             m03 * (m10 * m31 - m30 * m11));
01435         invertMatrix->m23 = -(
01436             m00 * (m11 * m23 - m21 * m13) -
01437             m01 * (m10 * m23 - m20 * m13) +
01438             m03 * (m10 * m21 - m20 * m11));
01439         invertMatrix->m31 = (
01440             m00 * (m21 * m32 - m31 * m22) -
01441             m01 * (m20 * m32 - m30 * m22) +
01442             m02 * (m20 * m31 - m30 * m21));
01443         invertMatrix->m32 = -(
01444             m00 * (m11 * m32 - m31 * m12) -
01445             m01 * (m10 * m32 - m30 * m12) +
01446             m02 * (m10 * m31 - m30 * m11));
01447         invertMatrix->m33 = (
01448             m00 * (m11 * m22 - m21 * m12) -
01449             m01 * (m10 * m22 - m20 * m12) +
01450             m02 * (m10 * m21 - m20 * m11));
01451         // 行列式の逆数を掛ける
01452         float invDeterm = 1.f / determ;
01453         (*invertMatrix) *= invDeterm;
01454         return determ;
01455     }
01456 
01457     //--------------------------------------------------------------------------
01458     /**
01459      * 変換行列の逆行列
01460      *
01461      * 変換行列には回転と移動しか含まれていないと仮定する
01462      */
01463     inline void invertTransformation(){
01464         Assert((m30 == 0.f) && (m31 == 0.f) && (m32 == 0.f) && (m33 == 1.f));
01465         // 回転行列を転置
01466         float swap;
01467         swap = m01; m01 = m10; m10 = swap;
01468         swap = m02; m02 = m20; m20 = swap;
01469         swap = m12; m12 = m21; m21 = swap;
01470         // 移動値を逆変換
01471         Vector3 trans(-m03, -m13, -m23);
01472         m03 = m00 * trans.x + m01 * trans.y + m02 * trans.z;
01473         m13 = m10 * trans.x + m11 * trans.y + m12 * trans.z;
01474         m23 = m20 * trans.x + m21 * trans.y + m22 * trans.z;
01475     }
01476 
01477     /**
01478      * 変換行列の逆行列
01479      *
01480      * 変換行列には回転と移動しか含まれていないと仮定する
01481      * @param invertMatrix [out] 逆行列を格納する行列へのポインタ
01482      */
01483     inline void invertTransformation(Matrix44* invertMatrix) const{
01484         Assert((m30 == 0.f) && (m31 == 0.f) && (m32 == 0.f) && (m33 == 1.f));
01485         // 回転行列を転置
01486         invertMatrix->m00 = m00;
01487         invertMatrix->m01 = m10;
01488         invertMatrix->m02 = m20;
01489         invertMatrix->m10 = m01;
01490         invertMatrix->m11 = m11;
01491         invertMatrix->m12 = m21;
01492         invertMatrix->m20 = m02;
01493         invertMatrix->m21 = m12;
01494         invertMatrix->m22 = m22;
01495         // 移動値を逆変換
01496         Vector3 trans(-m03, -m13, -m23);
01497         invertMatrix->m03 = m00 * trans.x + m10 * trans.y + m20 * trans.z;
01498         invertMatrix->m13 = m01 * trans.x + m11 * trans.y + m21 * trans.z;
01499         invertMatrix->m23 = m02 * trans.x + m12 * trans.y + m22 * trans.z;
01500         // 残りを埋めておく
01501         invertMatrix->m30 = 0.f;
01502         invertMatrix->m31 = 0.f;
01503         invertMatrix->m32 = 0.f;
01504         invertMatrix->m33 = 1.f;
01505     }
01506 
01507     //--------------------------------------------------------------------------
01508     // 論理演算
01509     //--------------------------------------------------------------------------
01510     /**
01511      * 行列が同じかどうか
01512      * @param target 比較する行列
01513      * @return 同じ値であればtrueを返す
01514      */
01515     inline bool operator ==(const Matrix44& target) const{
01516         return (
01517             (m00 == target.m00) && (m01 == target.m01) &&
01518             (m02 == target.m02) && (m03 == target.m03) &&
01519             (m10 == target.m10) && (m11 == target.m11) &&
01520             (m12 == target.m12) && (m13 == target.m13) &&
01521             (m20 == target.m20) && (m21 == target.m21) &&
01522             (m22 == target.m22) && (m23 == target.m23) &&
01523             (m30 == target.m30) && (m31 == target.m31) &&
01524             (m32 == target.m32) && (m33 == target.m33));
01525     }
01526 
01527     /**
01528      * 行列が同じかどうか
01529      * @param target 比較する行列
01530      * @param epsilon 誤差
01531      * @return 誤差の範囲内で同じ値であればtrueを返す
01532      */
01533     inline bool epsilonEquals(const Matrix44& target, float epsilon) const{
01534         Assert(epsilon >= 0.f);
01535         return (
01536             (Math::abs(m00 - target.m00) <= epsilon) &&
01537             (Math::abs(m01 - target.m01) <= epsilon) &&
01538             (Math::abs(m02 - target.m02) <= epsilon) &&
01539             (Math::abs(m03 - target.m03) <= epsilon) &&
01540             (Math::abs(m10 - target.m10) <= epsilon) &&
01541             (Math::abs(m11 - target.m11) <= epsilon) &&
01542             (Math::abs(m12 - target.m12) <= epsilon) &&
01543             (Math::abs(m13 - target.m13) <= epsilon) &&
01544             (Math::abs(m20 - target.m20) <= epsilon) &&
01545             (Math::abs(m21 - target.m21) <= epsilon) &&
01546             (Math::abs(m22 - target.m22) <= epsilon) &&
01547             (Math::abs(m23 - target.m23) <= epsilon) &&
01548             (Math::abs(m30 - target.m30) <= epsilon) &&
01549             (Math::abs(m31 - target.m31) <= epsilon) &&
01550             (Math::abs(m32 - target.m32) <= epsilon) &&
01551             (Math::abs(m33 - target.m33) <= epsilon));
01552     }
01553 
01554     /**
01555      * 行列が同じでないかどうか
01556      * @param target 比較する行列
01557      * @return 同じでない値であればtrueを返す
01558      */
01559     inline bool operator !=(const Matrix44& target) const{
01560         return (
01561             (m00 != target.m00) || (m01 != target.m01) ||
01562             (m02 != target.m02) || (m03 != target.m03) ||
01563             (m10 != target.m10) || (m11 != target.m11) ||
01564             (m12 != target.m12) || (m13 != target.m13) ||
01565             (m20 != target.m20) || (m21 != target.m21) ||
01566             (m22 != target.m22) || (m23 != target.m23) ||
01567             (m30 != target.m30) || (m31 != target.m31) ||
01568             (m32 != target.m32) || (m33 != target.m33));
01569     }
01570 
01571     /**
01572      * 行列が同じでないかどうか
01573      * @param target 比較する行列
01574      * @param epsilon 誤差
01575      * @return 誤差の範囲内で同じでない値であればtrueを返す
01576      */
01577     inline bool notEpsilonEquals(const Matrix44& target, float epsilon) const{
01578         Assert(epsilon >= 0.f);
01579         return (
01580             (Math::abs(m00 - target.m00) > epsilon) ||
01581             (Math::abs(m01 - target.m01) > epsilon) ||
01582             (Math::abs(m02 - target.m02) > epsilon) ||
01583             (Math::abs(m03 - target.m03) > epsilon) ||
01584             (Math::abs(m10 - target.m10) > epsilon) ||
01585             (Math::abs(m11 - target.m11) > epsilon) ||
01586             (Math::abs(m12 - target.m12) > epsilon) ||
01587             (Math::abs(m13 - target.m13) > epsilon) ||
01588             (Math::abs(m20 - target.m20) > epsilon) ||
01589             (Math::abs(m21 - target.m21) > epsilon) ||
01590             (Math::abs(m22 - target.m22) > epsilon) ||
01591             (Math::abs(m23 - target.m23) > epsilon) ||
01592             (Math::abs(m30 - target.m30) > epsilon) ||
01593             (Math::abs(m31 - target.m31) > epsilon) ||
01594             (Math::abs(m32 - target.m32) > epsilon) ||
01595             (Math::abs(m33 - target.m33) > epsilon));
01596     }
01597 
01598     //--------------------------------------------------------------------------
01599     // その他
01600     //--------------------------------------------------------------------------
01601     /**
01602      * 文字列化
01603      * @return 行列の文字列表記
01604      */
01605     inline String toString() const{
01606         String returnString;
01607         returnString.format(
01608             "{\n  ( %.8f, %.8f, %.8f, %.8f )\n  ( %.8f, %.8f, %.8f, %.8f )\n"
01609             "  ( %.8f, %.8f, %.8f, %.8f )\n  ( %.8f, %.8f, %.8f, %.8f )\n}",
01610             m00, m01, m02, m03, m10, m11, m12, m13,
01611             m20, m21, m22, m23, m30, m31, m32, m33);
01612         return returnString;
01613     }
01614 
01615     //--------------------------------------------------------------------------
01616 private:
01617 
01618 };
01619 
01620 //------------------------------------------------------------------------------
01621 } // End of namespace Lamp
01622 #endif // End of MATRIX44_H_
01623 //------------------------------------------------------------------------------

Generated on Wed Mar 16 10:29:32 2005 for Lamp by doxygen 1.3.2