예제 #1
0
파일: CMatrix.cs 프로젝트: tsu-kunn/TGA
        /// <summary>
        /// 逆行列の取得
        /// </summary>
        /// <param name="mat">出力先</param>
        /// <returns>逆行列がない場合は単位行列が代入され、falseが返る</returns>
        public bool Invers(CMatrix4 mat)
        {
            int   i, j, row;
            float tmp;

            float[,] mat84 = new float[4, 8];

            // 8x4行列に4x4行列と単位行列入れる
            for (i = 0; i < 4; i++)
            {
                for (j = 0; j < 4; j++)
                {
                    mat84[i, j] = m[i, j];

                    if (i == j)
                    {
                        mat84[i, (j + 4)] = 1.0f;
                    }
                    else
                    {
                        mat84[i, (j + 4)] = 0.0f;
                    }
                }
            }

            for (row = 0; row < 4; row++)
            {
                tmp = mat84[row, row];
                if (tmp != 1.0f)
                {
                    if (tmp == 0.0f)
                    {
                        for (i = row + 1; i < 4; i++)
                        {
                            tmp = mat84[i, row];
                            if (tmp != 0.0f)
                            {
                                break;
                            }
                        }

                        // 全て0なら逆行列なし
                        if (i >= 4)
                        {
                            mat.Identity(); // 単位行列を入れておく(保険)
                            return(false);
                        }

                        // 行を入れ替える
                        for (j = 0; j < 8; j++)
                        {
                            tmp           = mat84[i, j];
                            mat84[i, j]   = mat84[row, j];
                            mat84[row, j] = tmp;
                        }
                        tmp = mat84[row, row];
                    }

                    for (i = 0; i < 8; i++)
                    {
                        mat84[row, i] /= tmp;
                    }
                }

                // mat84[i][row]が1になるよう計算
                for (i = 0; i < 4; i++)
                {
                    if (i != row)
                    {
                        tmp = mat84[i, row];
                        if (tmp != 0)
                        {
                            for (j = 0; j < 8; j++)
                            {
                                mat84[i, j] -= mat84[row, j] * tmp;
                            }
                        }
                    }
                }
            }

            // 求まった逆行列を4x4行列にコピー
            for (i = 0; i < 4; i++)
            {
                for (j = 0; j < 4; j++)
                {
                    mat.m[i, j] = mat84[i, (j + 4)];
                }
            }

            return(true);
        }