GMatrix.cc

Go to the documentation of this file.

#include <vecmath/GMatrix.h>

using namespace std;

const double GMatrix::EPS = 1E-10;

GMatrix::GMatrix(int rows, int cols, const double ad[]) {
    nRow = rows;
    nCol = cols;
    values.resize(rows, cols);
    for (int i = 0; i < rows; i++)
        for (int j = 0; j < cols; j++)
            values[i][j] = ad[i * cols + j];
}

// extension
GMatrix::GMatrix(int rows, int cols, double *data, bool preset) {
    nRow = rows;
    nCol = cols;
    if (preset) {
        //cout << "Using preset: " << data << endl;
        // use preset directly !!!
        values.setPreset(cols, data);
    } else {
        values.resize(rows, cols);
        for (int i = 0; i < rows; i++)
            for (int j = 0; j < cols; j++)
                values[i][j] = data[i * cols + j];
    }
}

GMatrix::GMatrix(const GMatrix& gmatrix) {
    nRow = gmatrix.nRow;
    nCol = gmatrix.nCol;
    values.resize(nRow, nCol);
    for (int i = 0; i < nRow; i++)
        for (int j = 0; j < nCol; j++)
            values[i][j] = gmatrix.values[i][j];
}

int GMatrix::LUD(GMatrix& gmatrix, GVector& gvector) {
    int i = gmatrix.nRow * gmatrix.nCol;

    double *ad = new double[i];
    int *ai = new int[1];
    int *ai1 = new int[gmatrix.nRow];

    int idx = 0;
    for (int i = 0; i < nRow; i++)
        for (int j = 0; j < nCol; j++)
            ad[idx++] = values[i][j];

    idx = 0;
    for (int i = 0; i < nRow; i++)
        for (int j = 0; j < nCol; j++)
            gmatrix.values[i][j] = ad[idx++];

    for (int i = 0; i < gmatrix.nRow; i++)
        gvector.values[i] = ai1[i];

        delete []ad;
        delete []ai1;

        return ai[0];
}

// from - to, scale,  rot
int GMatrix::SVD(GMatrix& gmatrix, GMatrix& gmatrix1, GMatrix& gmatrix2) {
    return computeSVD(*this, gmatrix, gmatrix1, gmatrix2, false);
}

void GMatrix::add(const GMatrix & gmatrix) {
    for (int i = 0; i < nRow; i++)
        for (int j = 0; j < nCol; j++)
            values[i][j] += gmatrix.values[i][j];
}

void GMatrix::add(const GMatrix & gmatrix, const GMatrix & gmatrix1) {
    for (int i = 0; i < nRow; i++)
        for (int j = 0; j < nCol; j++)
            values[i][j] = gmatrix.values[i][j] + gmatrix1.values[i][j];
}

int GMatrix::computeSVD(GMatrix& gmatrix, GMatrix& gmatrix1, GMatrix& gmatrix2,
                        GMatrix& gmatrix3, bool flag) {
        std::cerr << "GMatrix.computeSVD FIXME" << std::endl;
    GMatrix gmatrix4(gmatrix.nRow, gmatrix.nCol);
    GMatrix gmatrix5(gmatrix.nRow, gmatrix.nCol);
    GMatrix gmatrix6(gmatrix.nRow, gmatrix.nCol);
    GMatrix gmatrix7(gmatrix);
    int i10;
    int j10;

    if (gmatrix7.nRow >= gmatrix7.nCol) {
        j10 = gmatrix7.nCol;
        i10 = gmatrix7.nCol - 1;
    } else {
        j10 = gmatrix7.nRow;
        i10 = gmatrix7.nRow;
    }

    double *ad = new double[(gmatrix7.nRow > gmatrix7.nCol) ? gmatrix7.nRow : gmatrix7.nCol];
    double *ad1 = new double[j10];
    double *ad2 = new double[i10];

    int ad2length = i10;

//    if (flag) cout << "input to compute_svd = " << endl << gmatrix7 << endl;
    int l9 = 0;

    gmatrix1.setIdentity();
    gmatrix3.setIdentity();
    int nRow = gmatrix7.nRow;
    int nCol = gmatrix7.nCol;

    for (int row = 0; row < j10; row++) {
        if (nRow > 1) {
            if (flag)
                std::cout << "************************ U ************************" << std::endl;

            double d = 0.0;

            for (int i = 0; i < nRow; i++) {
                d += Math::sqr(gmatrix7.values[i + row][row]);
                if (flag)
                    std::cout << "mag= " << d << " matrix.dot=" <<
                        gmatrix7.values[i + row][row] * gmatrix7.values[i + row][row] << std::endl;
            }

            d = sqrt(d);

            if (gmatrix7.values[row][row] == 0.0)
                ad[0] = d;
            else
                ad[0] = gmatrix7.values[row][row] + d_sign(d, gmatrix7.values[row][row]);
            for (int i = 1; i < nRow; i++)
                ad[i] = gmatrix7.values[row + i][row];

            double d2 = 0.0;

            for (int i = 0; i < nRow; i++) {
                if (flag)
                    std::cout << "vec[" << i << "]=" << ad[i] << std::endl;
                d2 += Math::sqr(ad[i]);
            }


            d2 = (d2 > 0.0) ? 2.0 / d2 : 0.0;

            if (flag)
                std::cout << "scale= " << d2 << std::endl;

            for (int i = row; i < gmatrix7.nRow; i++)
                for (int j = row; j < gmatrix7.nRow; j++)
                    gmatrix5.values[i][j] = -d2 * ad[i - row] * ad[j - row];

            for (int i = row; i < gmatrix7.nRow; i++)
                gmatrix5.values[i][i]++;

            double d4 = 0.0;

            for (int i = row; i < gmatrix7.nRow; i++)
                d4 += gmatrix5.values[row][i] * gmatrix7.values[i][row];

            gmatrix7.values[row][row] = d4;

            for (int i = row; i < gmatrix7.nRow; i++)
                for (int j = row + 1; j < gmatrix7.nCol; j++) {
                    double d = 0.0;
                    for (int k = row; k < gmatrix7.nCol; k++)
                        d += gmatrix5.values[i][k] * gmatrix7.values[k][j];
                    gmatrix4.values[i][j] = d;
                }

            for (int i = row; i < gmatrix7.nRow; i++)
                for (int j = row + 1; j < gmatrix7.nCol; j++)
                    gmatrix7.values[i][j] = gmatrix4.values[i][j];

            if (flag) {
//              cout << "U=" << endl << gmatrix1 << endl;
//              cout << "u=" << endl << gmatrix5 << endl;
            }

            for (int l4 = row; l4 < gmatrix7.nRow; l4++) {
                for (int j7 = 0; j7 < gmatrix7.nCol; j7++) {
                    double d = 0.0;
                    for (int k = row; k < gmatrix7.nCol; k++)
                        d += gmatrix5.values[l4][k] * gmatrix1.values[k][j7];
                    gmatrix4.values[l4][j7] = d;

                }

            }

            for (int i = row; i < gmatrix7.nRow; i++)
                for (int j = 0; j < gmatrix7.nCol; j++)
                    gmatrix1.values[i][j] = gmatrix4.values[i][j];

            if (flag) {
                    std::cout << "single_values[" << row << "]=" << std::endl << ad1[row] << std::endl;
//              cout << "m=" << endl << gmatrix7 << endl;
//              cout << "U=" << endl << gmatrix1 << endl;
            }
            nRow--;
        }
        if (nCol > 2) {
            if (flag)
                std::cout << "************************ V ************************" << std::endl << std::endl;
            double d1 = 0.0;

            for (int i = 1; i < nCol; i++) {
                    std::cout << i << ", " << row << ", " << nCol
                    << " -> " << gmatrix7.values[row][row + i] << std::endl;
                d1 += Math::sqr(gmatrix7.values[row][row + i]);
            }

            if (flag)
                cout << "mag = " << d1 << endl;

            d1 = sqrt(d1);

//            cout << gmatrix7 << endl;

            if (gmatrix7.values[row][row + 1] == 0.0)
                ad[0] = d1;
            else
                ad[0] = gmatrix7.values[row][row + 1] + d_sign(d1, gmatrix7.values[row][row + 1]);

            for (int i = 1; i < nCol - 1; i++)
                ad[i] = gmatrix7.values[row][row + i + 1];

            double d3 = 0.0;

            for (int i = 0; i < nCol - 1; i++) {
                if (flag)
                    cout << "vec[" << i << "]=" << ad[i] << endl;
                if (ad[i] > 0)
                    d3 += Math::sqr(ad[i]);
            }

            //???????????
            d3 = (d3 > 0.0) ? 2.0 / d3 : 0.0;

            if (flag)
                cout << "scale = " << d3 << endl;

            for (int i = row + 1; i < nCol; i++)
                for (int j = row + 1; j < gmatrix7.nCol; j++)
                    gmatrix6.values[i][j] = -d3 * ad[i - row - 1] * ad[j - row - 1];


            for (int i = row + 1; i < gmatrix7.nCol; i++)
                gmatrix6.values[i][i]++;

            double d5 = 0.0;

            for (int i = row; i < gmatrix7.nCol; i++)
                d5 += gmatrix6.values[i][row + 1] * gmatrix7.values[row][i];

            gmatrix7.values[row][row + 1] = d5;

            for (int i = row + 1; i < gmatrix7.nRow; i++) {
                for (int j = row + 1; j < gmatrix7.nCol; j++) {
                    double d = 0.0;
                    for (int k = row + 1; k < gmatrix7.nCol; k++)
                        d += gmatrix6.values[k][j] * gmatrix7.values[i][k];
                    gmatrix4.values[i][j] = d;
                }
            }

            for (int i = row + 1; i < gmatrix7.nRow; i++)
                for (int j = row + 1; j < gmatrix7.nCol; j++)
                    gmatrix7.values[i][j] = gmatrix4.values[i][j];

            if (flag)
//              cout << "V=" << endl << gmatrix3 << endl
//                  << "v=" << endl << gmatrix6 << endl
//                  << "tmp=" << endl << gmatrix4 << endl;

            for (int i = 0; i < gmatrix7.nRow; i++) {
                for (int j = row + 1; j < gmatrix7.nCol; j++) {
                    double d = 0.0;
                    for (int j3 = row + 1; j3 < gmatrix7.nCol; j3++)
                        d += gmatrix6.values[j3][j] * gmatrix3.values[i][j3];
                    gmatrix4.values[i][j] = d;
                }
            }

            if (flag)
//              cout << "tmp=" << endl << gmatrix4 << endl;

            for (int i = 0; i < gmatrix7.nRow; i++)
                for (int j = row + 1; j < gmatrix7.nCol; j++)
                    gmatrix3.values[i][j] = gmatrix4.values[i][j];


            if (flag)
//              cout << "m=" << endl << gmatrix7 << endl
//                  << "V=" << endl << gmatrix3 << endl;

            nCol--;
        }
    }

    for (int i = 0; i < j10; i++)
        ad1[i] = gmatrix7.values[i][i];

    for (int i = 0; i < i10; i++)
        ad2[i] = gmatrix7.values[i][i + 1];

    compute_qr(0, ad2length - 1, ad1, ad2, gmatrix1, gmatrix3, flag);
    
        delete []ad;
        delete []ad1;
        delete []ad2;

        //l9 = ad1.length;
    return l9;
}

int GMatrix::compute_2X2(double d, double d1, double d2, double ad[], double ad1[], double ad2[],
                       double ad3[], double ad4[], int i) {
    double d3 = 2;
    double d4 = 1.0;
    double d37 = ad[0];
    double d36 = ad[1];
    double d32 = 0.0;
    double d33 = 0.0;
    double d34 = 0.0;
    double d35 = 0.0;
    double d21 = 0.0;
    double d25 = d;
    double d22 = Math::abs(d25);
    double d27 = d2;
    double d24 = Math::abs(d2);
    byte byte0 = 1;

    bool flag = (d24 > d22);

    if (flag) {
        byte0 = 3;
        double d6 = d25;
        d25 = d27;
        d27 = d6;
        d6 = d22;
        d22 = d24;
        d24 = d6;
    }

    double d26 = d1;
    double d23 = Math::abs(d26);

    if (d23 == 0.0) {
        ad[1] = d24;
        ad[0] = d22;
        d32 = 1.0;
        d33 = 1.0;
        d34 = 0.0;
        d35 = 0.0;
    } else {
        bool flag1 = true;

        if (d23 > d22) {
            byte0 = 2;
            if (d22 / d23 < 1E-10) {
                flag1 = false;
                d37 = d23;
                if (d24 > 1.0)
                    d36 = d22 / (d23 / d24);
                else
                    d36 = (d22 / d23) * d24;
                d32 = 1.0;
                d34 = d27 / d26;
                d35 = 1.0;
                d33 = d25 / d26;
            }
        }
        if (flag1) {
            double d9 = d22 - d24;
            double d11;

            if (d9 == d22)
                d11 = 1.0;
            else
                d11 = d9 / d22;
            double d13 = d26 / d25;
            //double d19 = 2 - d11; unused
            double d28 = d13 * d13;
            //double d30 = d19 * d19;
            //double d17 = sqrt(d30 + d28);
            double d15;

            if (d11 == 0.0)
                d15 = Math::abs(d13);
            else
                d15 = sqrt(d11 * d11 + d28);
            //double d7 = (d17 + d15) * 0.5;

            if (d23 > d22) {
                byte0 = 2;
                if (d22 / d23 < 1E-10) {
                    flag1 = false;
                    d37 = d23;
                    if (d24 > 1.0)
                        d36 = d22 / (d23 / d24);
                    else
                        d36 = (d22 / d23) * d24;
                    d32 = 1.0;
                    d34 = d27 / d26;
                    d35 = 1.0;
                    d33 = d25 / d26;
                }
            }
            if (flag1) {
                double d10 = d22 - d24;
                double d12;

                if (d10 == d22)
                    d12 = 1.0;
                else
                    d12 = d10 / d22;
                double d14 = d26 / d25;
                double d20 = 2 - d12;
                double d29 = d14 * d14;
                double d31 = d20 * d20;
                double d18 = sqrt(d31 + d29);
                double d16;

                if (d12 == 0.0)
                    d16 = Math::abs(d14);
                else
                    d16 = sqrt(d12 * d12 + d29);
                double d8 = (d18 + d16) * 0.5;

                d36 = d24 / d8;
                d37 = d22 * d8;
                if (d29 == 0.0) {
                    if (d12 == 0.0)
                        d20 = d_sign(d3, d25) * d_sign(d4, d26);
                    else
                        d20 = d26 / d_sign(d10, d25) + d14 / d20;
                } else {
                    d20 = (d14 / (d18 + d20) + d14 / (d16 + d12)) * (d8 + 1.0);
                }
                d12 = sqrt(d20 * d20 + 4);
                d33 = 2 / d12;
                d35 = d20 / d12;
                d32 = (d33 + d35 * d14) / d8;
                d34 = ((d27 / d25) * d35) / d8;
            }
        }
        if (flag) {
            ad2[0] = d35;
            ad1[0] = d33;
            ad4[0] = d34;
            ad3[0] = d32;
        } else {
            ad2[0] = d32;
            ad1[0] = d34;
            ad4[0] = d33;
            ad3[0] = d35;
        }
        if (byte0 == 1)
            d21 = d_sign(d4, ad4[0]) * d_sign(d4, ad2[0]) * d_sign(d4, d);
        if (byte0 == 2)
            d21 = d_sign(d4, ad3[0]) * d_sign(d4, ad2[0]) * d_sign(d4, d1);
        if (byte0 == 3)
            d21 = d_sign(d4, ad3[0]) * d_sign(d4, ad1[0]) * d_sign(d4, d2);
        ad[i] = d_sign(d37, d21);
        double d5 = d21 * d_sign(d4, d) * d_sign(d4, d2);

        ad[i + 1] = d_sign(d36, d5);
    }
    return 0;
}



/*------------------------------------------------------------------------
 *              QR-diagonalization of a bidiagonal matrix
 *
 * After bidiagonalization we get a bidiagonal matrix J:
 *    (1)  J = U' * A * V
 * The present method turns J into a matrix JJ by applying a set of
 * orthogonal transforms
 *    (2)  JJ = S' * J * T
 * Orthogonal matrices S and T are chosen so that JJ were also a
 * bidiagonal matrix, but with superdiag elements smaller than those of J.
 * We repeat (2) until non-diag elements of JJ become smaller than EPS
 * and can be disregarded.
 * Matrices S and T are constructed as
 *    (3)  S = S1 * S2 * S3 ... Sn, and similarly T
 * where Sk and Tk are matrices of simple rotations
 *    (4)  Sk[i,j] = i==j ? 1 : 0 for all i>k or i<k-1
 *         Sk[k-1,k-1] = cos(Phk),  Sk[k-1,k] = -sin(Phk),
 *         SK[k,k-1] = sin(Phk),    Sk[k,k] = cos(Phk), k=2..N
 * Matrix Tk is constructed similarly, only with angle Thk rather than Phk.
 *
 * Thus left multiplication of J by SK' can be spelled out as
 *    (5)  (Sk' * J)[i,j] = J[i,j] when i>k or i<k-1,
 *                  [k-1,j] = cos(Phk)*J[k-1,j] + sin(Phk)*J[k,j]
 *                  [k,j] =  -sin(Phk)*J[k-1,j] + cos(Phk)*J[k,j]
 * That is, k-1 and k rows of J are replaced by their linear combinations;
 * the rest of J is unaffected. Right multiplication of J by Tk similarly
 * changes only k-1 and k columns of J.
 * Matrix T2 is chosen the way that T2'J'JT2 were a QR-transform with a
 * shift. Note that multiplying J by T2 gives rise to a J[2,1] element of
 * the product J (which is below the main diagonal). Angle Ph2 is then
 * chosen so that multiplication by S2' (which combines 1 and 2 rows of J)
 * gets rid of that elemnent. But this will create a [1,3] non-zero element.
 * T3 is made to make it disappear, but this leads to [3,2], etc.
 * In the end, Sn removes a [N,N-1] element of J and matrix S'JT becomes
 * bidiagonal again. However, because of a special choice
 * of T2 (QR-algorithm), its non-diag elements are smaller than those of J.
 *
 * All this process in more detail is described in
 *    J.H. Wilkinson, C. Reinsch. Linear algebra - Springer-Verlag, 1971
 *
 * If during transforms (1), JJ[N-1,N] turns 0, then JJ[N,N] is a singular
 * number (possibly with a wrong (that is, negative) sign). This is a
 * consequence of Frantsis' Theorem, see the book above. In that case, we can
 * eliminate the N-th row and column of JJ and carry out further transforms
 * with a smaller matrix. If any other superdiag element of JJ turns 0,
 * then JJ effectively falls into two independent matrices. We will process
 * them independently (the bottom one first).
 *
 * Since matrix J is a bidiagonal, it can be stored efficiently. As a matter
 * of fact, its N diagonal elements are in array Sig, and superdiag elements
 * are stored in array super_diag.
 */

void GMatrix::compute_qr(int i, int j, double ad[], double ad1[], GMatrix& rot,
                         GMatrix& scale, bool flag) {
    double ad2[1];
    double ad3[1];
    double ad4[1];
    double ad5[1];
    GMatrix gmatrix2(rot.nCol, scale.nRow);
    //byte byte0 = 2;
    const double EPS = 4.89E-15;

    cerr << "GMatrix.compute_qr FIXME" << endl;
    if (flag) {
        cout << "start=" << endl << i << endl;
        cout << "s=" << endl;
        //          for (int k = 0; k < ad.length; k++)
        //              cout << ad[k];
        //
        //          cout << "es=" << endl;
        //          for (int l = 0; l < ad1.length; l++)
        //              cout << ad1[l];
        //
        //          for (int i1 = 0; i1 < ad.length; i1++)
        //              gmatrix2.values[i1][i1] = ad[i1];
        //
        //          for (int j1 = 0; j1 < ad1.length; j1++)
        //          gmatrix2.values[j1][j1 + 1] = ad1[j1];

//      cout << endl << "m=" << endl << gmatrix2 << endl;
    }
    double d6 = 1.0;
    //double d7 = -1;
    bool flag1 = false;

    if (flag)
        print_svd(ad, ad1, rot, scale);
    double d3 = 0.0;
    double d4 = 0.0;

    for (int i2 = 0; i2 < 2 && !flag1; i2++) {
        int k1;

        for (k1 = i; k1 <= j; k1++) {
            if (k1 == i) {
                int k2 = j;
                // FIXME
                //                  if (ad1.length == ad.length)
                //                      k2 = j;
                //                  else
                //                      k2 = j + 1;
                double d = compute_shift(ad[k2 - 1], ad1[j], ad[k2]);

                d3 = (Math::abs(ad[k1]) - d) * (d_sign(d6, ad[k1]) + d / ad[k1]);
                d4 = ad1[k1];
            }
            double d1 = compute_rot(d3, d4, ad5, ad3);

            if (k1 != i)
                ad1[k1 - 1] = d1;
            d3 = ad3[0] * ad[k1] + ad5[0] * ad1[k1];
            ad1[k1] = ad3[0] * ad1[k1] - ad5[0] * ad[k1];
            d4 = ad5[0] * ad[k1 + 1];
            ad[k1 + 1] = ad3[0] * ad[k1 + 1];
            update_v(k1, scale, ad3, ad5);
            if (flag)
                print_m(gmatrix2, rot, scale);
            d1 = compute_rot(d3, d4, ad4, ad2);
            ad[k1] = d1;
            d3 = ad2[0] * ad1[k1] + ad4[0] * ad[k1 + 1];
            ad[k1 + 1] = ad2[0] * ad[k1 + 1] - ad4[0] * ad1[k1];
            if (k1 < j) {
                d4 = ad4[0] * ad1[k1 + 1];
                ad1[k1 + 1] = ad2[0] * ad1[k1 + 1];
            }
            update_u(k1, rot, ad2, ad4);
            if (flag)
                print_m(gmatrix2, rot, scale);
        }

//      if (ad.length == ad1.length) {
            compute_rot(d3, d4, ad5, ad3);

            d3 = ad3[0] * ad[k1] + ad5[0] * ad1[k1];
            ad1[k1] = ad3[0] * ad1[k1] - ad5[0] * ad[k1];
            ad[k1 + 1] = ad3[0] * ad[k1 + 1];
            update_v(k1, scale, ad3, ad5);
            if (flag)
                print_m(gmatrix2, rot, scale);
//      }
        if (flag) {
            cout << endl << "*********************** iteration #" << i2 <<
                " ***********************" << endl << endl;
            print_svd(ad, ad1, rot, scale);
        }
        for (; j - i > 1 && Math::abs(ad1[j]) < EPS; j--) ;
        for (int j2 = j - 2; j2 > i; j2--)
            if (Math::abs(ad1[j2]) < EPS) {
                compute_qr(j2 + 1, j, ad, ad1, rot, scale, flag);
                for (j = j2 - 1; j - i > 1 && Math::abs(ad1[j]) < EPS;
                     j--) ;
            }

        if (flag)
            cout << "start = " << i << endl;
        if (j - i <= 1 && Math::abs(ad1[i + 1]) < EPS)
            flag1 = true;
    }

    if (flag)
        cout << endl << "****call compute_2X2 **********************" << endl;
    if (Math::abs(ad1[1]) < EPS) {
        compute_2X2(ad[i], ad1[i], ad[i + 1], ad, ad4, ad2, ad5, ad3, 0);
        ad1[i] = 0.0;
        ad1[i + 1] = 0.0;
    }
    int l1 = i;

    update_u(l1, rot, ad2, ad4);
    update_v(l1, scale, ad3, ad5);
    if (flag) {
        cout << endl << "*******after call compute_2X2 **********************" << endl;
        print_svd(ad, ad1, rot, scale);
    }
}

double GMatrix::compute_rot(double d, double d1, double ad[], double ad1[]) {
    const double d8 = 2.0020830951831009E-146;
    const double d9 = 4.9947976805055876E+145;
    double d2;
    double d3;
    double d7;
    if (d1 == 0.0) {
        d2 = 1.0;
        d3 = 0.0;
        d7 = d;
    } else if (d == 0.0) {
        d2 = 0.0;
        d3 = 1.0;
        d7 = d1;
    } else {
        double d5 = d;
        double d6 = d1;
        double d4 = Math::max(Math::abs(d5), Math::abs(d6));

        if (d4 >= d9) {
            int i1 = 0;

            for (; d4 >= d9; d4 = Math::max(Math::abs(d5), Math::abs(d6))) {
                i1++;
                d5 *= d8;
                d6 *= d8;
            }

            d7 = sqrt(d5 * d5 + d6 * d6);
            d2 = d5 / d7;
            d3 = d6 / d7;

            for (int i = 1; i <= i1; i++)
                d7 *= d9;

        } else if (d4 <= d8) {
            int j1 = 0;

            for (; d4 <= d8; d4 = Math::max(Math::abs(d5), Math::abs(d6))) {
                j1++;
                d5 *= d9;
                d6 *= d9;
            }

            d7 = sqrt(d5 * d5 + d6 * d6);
            d2 = d5 / d7;
            d3 = d6 / d7;

            for (int i = 1; i <= j1; i++)
                d7 *= d8;

        } else {
            d7 = sqrt(d5 * d5 + d6 * d6);
            d2 = d5 / d7;
            d3 = d6 / d7;
        }
        if (Math::abs(d) > Math::abs(d1) && d2 < 0.0) {
            d2 = -d2;
            d3 = -d3;
            d7 = -d7;
        }
    }
    ad[0] = d3;
    ad1[0] = d2;
    return d7;
}

double GMatrix::compute_shift(double d, double d1, double d2) {
    double d11 = Math::abs(d);
    double d12 = Math::abs(d1);
    double d13 = Math::abs(d2);
    double d7 = Math::min(d11, d13);
    double d8 = Math::max(d11, d13);
    double d20;

    if (d7 == 0.0) {
        d20 = 0.0;
        double d3;
        if (d8 != 0.0)
            d3 = Math::min(d8, d12) / Math::max(d8, d12);
    } else if (d12 < d8) {
        double d14 = d7 / d8 + 1.0;
        double d16 = (d8 - d7) / d8;
        double d4 = d12 / d8;
        double d18 = d4 * d4;
        double d9 = 2 / (sqrt(d14 * d14 + d18) + sqrt(d16 * d16 + d18));

        d20 = d7 * d9;
    } else {
        double d19 = d8 / d12;

        if (d19 == 0.0) {
            d20 = (d7 * d8) / d12;
        } else {
            double d15 = d7 / d8 + 1.0;
            double d17 = (d8 - d7) / d8;
            double d5 = d15 * d19;
            double d6 = d17 * d19;
            double d10 = 1.0 / (sqrt(d5 * d5 + 1.0) + sqrt(d6 * d6 + 1.0));

            d20 = d7 * d10 * d19;
            d20 += d20;
        }
    }
    return d20;
}

void GMatrix::copySubMatrix(int i, int j, int k, int l, int i1, int j1, GMatrix& gmatrix) {
    if (this != &gmatrix) {
        for (int k1 = 0; k1 < k; k1++)
            for (int j2 = 0; j2 < l; j2++)
                gmatrix.values[i1 + k1][j1 + j2] = values[i + k1][j + j2];
    } else {
        DoubleArray2 ad(k, l);

        for (int r = 0; r < k; r++)
            for (int s = 0; s < l; s++)
                ad[r][s] = values[i + r][j + s];

        for (int i2 = 0; i2 < k; i2++)
            for (int l2 = 0; l2 < l; l2++)
                gmatrix.values[i1 + i2][j1 + l2] = ad[i2][l2];
    }
}

bool GMatrix::epsilonEquals(const GMatrix & gmatrix, double d) const {
    if (nRow != gmatrix.nRow || nCol != gmatrix.nCol)
        return false;

    for (int i = 0; i < nRow; i++) {
        for (int j = 0; j < nCol; j++) {
            double d1 = values[i][j] - gmatrix.values[i][j];
            if ((d1 >= 0.0 ? d1 : -d1) > d)
                return false;
        }
    } return true;
}

bool GMatrix::equals(const GMatrix & gmatrix) const {
    if (nRow != gmatrix.nRow || nCol != gmatrix.nCol)
        return false;

    for (int i = 0; i < nRow; i++)
        for (int j = 0; j < nCol; j++)
            if (values[i][j] != gmatrix.values[i][j])
                return false;

    return true;
}

void GMatrix::get(GMatrix& gmatrix) const {
    int k1;
    if (nCol < gmatrix.nCol)
        k1 = nCol;
    else
        k1 = gmatrix.nCol;

    int l1;
    if (nRow < gmatrix.nRow)
        l1 = nRow;
    else
        l1 = gmatrix.nRow;

    for (int i = 0; i < l1; i++)
        for (int l = 0; l < k1; l++)
            gmatrix.values[i][l] = values[i][l];

    for (int j = l1; j < gmatrix.nRow; j++)
        for (int i1 = 0; i1 < gmatrix.nCol; i1++)
            gmatrix.values[j][i1] = 0.0;

    for (int j1 = k1; j1 < gmatrix.nCol; j1++)
        for (int k = 0; k < l1; k++)
            gmatrix.values[k][j1] = 0.0;
}

void GMatrix::get(Matrix3d& m) const {
    if (nRow < 3 || nCol < 3) {
        for (int i = 0; i < 3; i++) {
            for (int j = 0; j < 3; j++)
                if (i < nRow && j < nCol)
                    m(i, j) = values[i][j];
                else
                    m(i, j) = 0.0;
        }
    } else {
        m.m00 = values[0][0];
        m.m01 = values[0][1];
        m.m02 = values[0][2];
        m.m10 = values[1][0];
        m.m11 = values[1][1];
        m.m12 = values[1][2];
        m.m20 = values[2][0];
        m.m21 = values[2][1];
        m.m22 = values[2][2];
    }
}

void GMatrix::get(Matrix3f& m) const {
    if (nRow < 3 || nCol < 3) {
        for (int i = 0; i < 3; i++) {
            for (int j = 0; j < 3; j++)
                if (i < nRow && j < nCol)
                    m(i, j) = values[i][j];
                else
                    m(i, j) = 0.0;
        }
    } else {
        m.m00 = values[0][0];
        m.m01 = values[0][1];
        m.m02 = values[0][2];
        m.m10 = values[1][0];
        m.m11 = values[1][1];
        m.m12 = values[1][2];
        m.m20 = values[2][0];
        m.m21 = values[2][1];
        m.m22 = values[2][2];
    }
}

void GMatrix::get(Matrix4d& m) const {
    if (nRow < 4 || nCol < 4) {
        for (int i = 0; i < 4; i++) {
            for (int j = 0; j < 4; j++)
                if (i < nRow && j < nCol)
                    m(i, j) = values[i][j];
                else
                    m(i, j) = 0.0;
        }
    } else {
        m.m00 = values[0][0];
        m.m01 = values[0][1];
        m.m02 = values[0][2];
        m.m03 = values[0][3];
        m.m10 = values[1][0];
        m.m11 = values[1][1];
        m.m12 = values[1][2];
        m.m13 = values[1][3];
        m.m20 = values[2][0];
        m.m21 = values[2][1];
        m.m22 = values[2][2];
        m.m23 = values[2][3];
        m.m30 = values[3][0];
        m.m31 = values[3][1];
        m.m32 = values[3][2];
        m.m33 = values[3][3];
    }
}

void GMatrix::get(Matrix4f& m) const {
    if (nRow < 4 || nCol < 4) {
        for (int i = 0; i < 4; i++) {
            for (int j = 0; j < 4; j++)
                if (i < nRow && j < nCol)
                    m(i, j) = values[i][j];
                else
                    m(i, j) = 0.0;
        }
    } else {
        m.m00 = values[0][0];
        m.m01 = values[0][1];
        m.m02 = values[0][2];
        m.m03 = values[0][3];
        m.m10 = values[1][0];
        m.m11 = values[1][1];
        m.m12 = values[1][2];
        m.m13 = values[1][3];
        m.m20 = values[2][0];
        m.m21 = values[2][1];
        m.m22 = values[2][2];
        m.m23 = values[2][3];
        m.m30 = values[3][0];
        m.m31 = values[3][1];
        m.m32 = values[3][2];
        m.m33 = values[3][3];
    }
}

void GMatrix::identityMinus() {
    for (int i = 0; i < nRow; i++)
        for (int k = 0; k < nCol; k++)
            values[i][k] = -values[i][k];

    int l = (nRow < nCol) ? nRow : nCol;
    for (int i = 0; i < l; i++)
        values[i][i]++;
}

void GMatrix::luBacksubstitution(int i, double ad[], int ai[], double ad1[]) {
    int i2 = 0;
    for (int l1 = 0; l1 < i; l1++) {
        int j2 = l1;
        int l = -1;
        for (int j = 0; j < i; j++) {
            int i1 = ai[i2 + j];
            double d1 = ad1[j2 + i * i1];
            ad1[j2 + i * i1] = ad1[j2 + i * j];
            if (l >= 0) {
                int k2 = j * i;
                for (int j1 = l; j1 <= j - 1; j1++)
                    d1 -= ad[k2 + j1] * ad1[j2 + i * j1];

            } else if (d1 != 0.0)
                l = j;

            ad1[j2 + i * j] = d1;
        }

        for (int k = 0; k < i; k++) {
            int i3 = i - 1 - k;
            int l2 = i * i3;
            double d = 0.0;

            for (int k1 = 1; k1 <= k; k1++)
                d += ad[(l2 + i) - k1] * ad1[j2 + i * (i - k1)];

            ad1[j2 + i * i3] = (ad1[j2 + i * i3] - d) / ad[l2 + i3];
        }
    }
}

bool GMatrix::luDecomposition(int i, double ad[], int ai[], int ai1[]) {
    double *ad1 = new double[i];
    int l = 0;
    int i1 = 0;
    ai1[0] = 1;
    for (int j = i; j-- != 0;) {
        double d = 0.0;
        for (int k = i; k-- != 0;) {
            double d1 = ad[l++];
            d1 = Math::abs(d1);
            if (d1 > d)
                d = d1;
        }

        if (d == 0.0)
            return false;

        ad1[i1++] = 1.0 / d;
    }

    int k1 = 0;

    for (int j1 = 0; j1 < i; j1++) {
        for (int l1 = 0; l1 < j1; l1++) {
            int k3 = k1 + i * l1 + j1;
            double d2 = ad[k3];
            int l2 = l1;
            int j4 = k1 + i * l1;

            for (int i5 = k1 + j1; l2-- != 0; i5 += i) {
                d2 -= ad[j4] * ad[i5];
                j4++;
            }

            ad[k3] = d2;
        }

        double d4 = 0.0;
        int k2 = -1;

        for (int i2 = j1; i2 < i; i2++) {
            int l3 = k1 + i * i2 + j1;
            double d3 = ad[l3];
            int i3 = j1;
            int k4 = k1 + i * i2;

            for (int j5 = k1 + j1; i3-- != 0; j5 += i) {
                d3 -= ad[k4] * ad[j5];
                k4++;
            }

            ad[l3] = d3;
            double d5;

            if ((d5 = ad1[i2] * Math::abs(d3)) >= d4) {
                d4 = d5;
                k2 = i2;
            }
        }

        if (j1 != k2) {
            int j3 = i;
            int l4 = k1 + i * k2;
            int k5 = k1 + i * j1;

            while (j3-- != 0) {
                double d6 = ad[l4];

                ad[l4++] = ad[k5];
                ad[k5++] = d6;
            }
            ad1[k2] = ad1[j1];
            ai1[0] = -ai1[0];
        }
        ai[j1] = k2;
        if (ad[k1 + i * j1 + j1] == 0.0) {
                delete []ad1;
            return false;
        }
        if (j1 != i - 1) {
            double d7 = 1.0 / ad[k1 + i * j1 + j1];
            int i4 = k1 + i * (j1 + 1) + j1;

            for (int j2 = i - 1 - j1; j2-- != 0;) {
                ad[i4] *= d7;
                i4 += i;
            }
        }
    }

        delete []ad1;
    return true;
}

void GMatrix::mul(const GMatrix& gmatrix) {
    DoubleArray2 ad(nRow, nCol);
    for (int i = 0; i < nRow; i++) {
        for (int j = 0; j < nCol; j++) {
            double d = 0.0;
            for (int k = 0; k < nCol; k++)
                d += values[i][k] * gmatrix.values[k][j];
            ad[i][j] = d;
        }
    }
    values.swap(ad);
}

void GMatrix::mul(const GMatrix& gmatrix, const GMatrix& gmatrix1) {
    DoubleArray2 ad(nRow, nCol);
    for (int i = 0; i < gmatrix.nRow; i++) {
        for (int j = 0; j < gmatrix1.nCol; j++) {
            double d = 0.0;
            for (int k = 0; k < gmatrix.nCol; k++)
                d += gmatrix.values[i][k] * gmatrix1.values[k][j];
            ad[i][j] = d;
        }
    }
    values.swap(ad);
}

void GMatrix::mul(const GVector& gvector, const GVector& gvector1) {
    for (int i = 0; i < gvector.getSize(); i++) {
        for (int j = 0; j < gvector1.getSize(); j++)
            values[i][j] = gvector.values[i] * gvector1.values[j];
    }
}

void GMatrix::mulTransposeBoth(const GMatrix& gmatrix, const GMatrix& gmatrix1) {
    if (&gmatrix == this || &gmatrix1 == this) {
        DoubleArray2 ad(nRow, nCol);
        for (int i = 0; i < nRow; i++) {
            for (int j = 0; j < nCol; j++) {
                double d = 0.0;
                for (int k = 0; k < gmatrix.nRow; k++)
                    d += gmatrix.values[k][i] * gmatrix1.values[j][k];
                ad[i][j] = d;
            }
        }
        values.swap(ad);
    } else {
        for (int i = 0; i < nRow; i++) {
            for (int j = 0; j < nCol; j++) {
                double d = 0.0;
                for (int k = 0; k < gmatrix.nRow; k++)
                    d += gmatrix.values[k][i] * gmatrix1.values[j][k];
                values[i][j] = d;
            }
        }
    }
}

void GMatrix::mulTransposeLeft(GMatrix gmatrix, GMatrix gmatrix1) {
    if (&gmatrix == this || &gmatrix1 == this) {
        DoubleArray2 ad(nRow, nCol);

        for (int i = 0; i < nRow; i++) {
            for (int j = 0; j < nCol; j++) {
                double d = 0.0;
                for (int k = 0; k < gmatrix.nRow; k++)
                    d += gmatrix.values[k][i] * gmatrix1.values[k][j];
                ad[i][j] = d;
            }
        }
        values.swap(ad);
    } else {
        for (int i = 0; i < nRow; i++) {
            for (int j = 0; j < nCol; j++) {
                double d = 0.0;
                for (int k = 0; k < gmatrix.nRow; k++)
                    d += gmatrix.values[k][i] * gmatrix1.values[k][j];
                values[i][j] = d;
            }
        }
    }
}

void GMatrix::mulTransposeRight(const GMatrix& gmatrix, const GMatrix& gmatrix1) {
    if (&gmatrix == this || &gmatrix1 == this) {
        DoubleArray2 ad(nRow, nCol);
        for (int i = 0; i < nRow; i++) {
            for (int k = 0; k < nCol; k++) {
                ad[i][k] = 0.0;
                for (int i1 = 0; i1 < gmatrix.nCol; i1++)
                    ad[i][k] += gmatrix.values[i][i1] * gmatrix1.values[k][i1];

            }
        }
        values.swap(ad);
    } else {
        for (int j = 0; j < nRow; j++) {
            for (int l = 0; l < nCol; l++) {
                values[j][l] = 0.0;
                for (int j1 = 0; j1 < gmatrix.nCol; j1++)
                    values[j][l] += gmatrix.values[j][j1] * gmatrix1.values[l][j1];
            }
        }
    }
}

void GMatrix::negate() {
    for (int i = 0; i < nRow; i++)
        for (int j = 0; j < nCol; j++)
            values[i][j] = -values[i][j];
}

void GMatrix::negate(const GMatrix& gmatrix) {
    for (int i = 0; i < nRow; i++)
        for (int j = 0; j < nCol; j++)
            values[i][j] = -gmatrix.values[i][j];
}


void GMatrix::set(const GMatrix& gmatrix) {
    if (nRow < gmatrix.nRow || nCol < gmatrix.nCol) {
        nRow = gmatrix.nRow;
        nCol = gmatrix.nCol;
        values.resize(nRow, nCol);
    }

    for (int i = 0; i < Math::min(nRow, gmatrix.nRow); i++) {
        for (int k = 0; k < Math::min(nCol, gmatrix.nCol); k++)
            values[i][k] = gmatrix.values[i][k];
    }

    for (int j = gmatrix.nRow; j < nRow; j++) {
        for (int l = gmatrix.nCol; l < nCol; l++)
            values[j][l] = 0.0;
    }
}

void GMatrix::set(const Matrix3d& matrix3d) {
    resizeSet(3, 3);

    values[0][0] = matrix3d.m00;
    values[0][1] = matrix3d.m01;
    values[0][2] = matrix3d.m02;
    values[1][0] = matrix3d.m10;
    values[1][1] = matrix3d.m11;
    values[1][2] = matrix3d.m12;
    values[2][0] = matrix3d.m20;
    values[2][1] = matrix3d.m21;
    values[2][2] = matrix3d.m22;

#define CLEAN_REST(X) \
    do {\
    for (int i = X; i < nRow; i++)\
    for (int j = X; j < nCol; j++)\
    values[i][j] = 0.0;\
    } while(0)
}

void GMatrix::set(const Matrix3f& matrix3f) {
    resizeSet(3, 3);

    values[0][0] = matrix3f.m00;
    values[0][1] = matrix3f.m01;
    values[0][2] = matrix3f.m02;
    values[1][0] = matrix3f.m10;
    values[1][1] = matrix3f.m11;
    values[1][2] = matrix3f.m12;
    values[2][0] = matrix3f.m20;
    values[2][1] = matrix3f.m21;
    values[2][2] = matrix3f.m22;

    CLEAN_REST(3);
}

void GMatrix::set(const Matrix4d& matrix4d) {
    resizeSet(4, 4);

    values[0][0] = matrix4d.m00;
    values[0][1] = matrix4d.m01;
    values[0][2] = matrix4d.m02;
    values[0][3] = matrix4d.m03;
    values[1][0] = matrix4d.m10;
    values[1][1] = matrix4d.m11;
    values[1][2] = matrix4d.m12;
    values[1][3] = matrix4d.m13;
    values[2][0] = matrix4d.m20;
    values[2][1] = matrix4d.m21;
    values[2][2] = matrix4d.m22;
    values[2][3] = matrix4d.m23;
    values[3][0] = matrix4d.m30;
    values[3][1] = matrix4d.m31;
    values[3][2] = matrix4d.m32;
    values[3][3] = matrix4d.m33;

    CLEAN_REST(4);
}

void GMatrix::set(const Matrix4f& matrix4f) {
    resizeSet(4, 4);

    values[0][0] = matrix4f.m00;
    values[0][1] = matrix4f.m01;
    values[0][2] = matrix4f.m02;
    values[0][3] = matrix4f.m03;
    values[1][0] = matrix4f.m10;
    values[1][1] = matrix4f.m11;
    values[1][2] = matrix4f.m12;
    values[1][3] = matrix4f.m13;
    values[2][0] = matrix4f.m20;
    values[2][1] = matrix4f.m21;
    values[2][2] = matrix4f.m22;
    values[2][3] = matrix4f.m23;
    values[3][0] = matrix4f.m30;
    values[3][1] = matrix4f.m31;
    values[3][2] = matrix4f.m32;
    values[3][3] = matrix4f.m33;

    CLEAN_REST(4);
}

void GMatrix::set(const double ad[]) {
    for (int i = 0; i < nRow; i++)
        for (int j = 0; j < nCol; j++)
            values[i][j] = ad[nCol * i + j];
}

void GMatrix::setColumn(int i, const GVector& gvector) {
    for (int j = 0; j < nRow; j++)
        values[j][i] = gvector.values[j];
}

void GMatrix::setColumn(int i, const double ad[]) {
    for (int j = 0; j < nRow; j++)
        values[j][i] = ad[j];
}

void GMatrix::setRow(int i, const GVector& gvector) {
    for (int j = 0; j < nCol; j++)
        values[i][j] = gvector.values[j];
}

void GMatrix::setRow(int i, const double ad[]) {
    for (int j = 0; j < nCol; j++)
        values[i][j] = ad[j];
}

void GMatrix::setScale(double d) {
    setZero();
    int l = (nRow < nCol) ? nRow : nCol;
    for (int i = 0; i < l; i++)
        values[i][i] = d;
}

void GMatrix::setSize(int i, int j) {
    DoubleArray2 ad(i, j);

    int i1 = (nRow < i) ? nRow : i;
    int j1 = (nCol < j) ? nCol : j;

    for (int k = 0; k < i1; k++)
        for (int l = 0; l < j1; l++)
            ad[k][l] = values[k][l];

    nRow = i;
    nCol = j;

    values.swap(ad);
}

void GMatrix::sub(const GMatrix& gmatrix) {
    for (int i = 0; i < nRow; i++)
        for (int j = 0; j < nCol; j++)
            values[i][j] -= gmatrix.values[i][j];
}

void GMatrix::sub(const GMatrix& gmatrix, const GMatrix& gmatrix1) {
    for (int i = 0; i < nRow; i++)
        for (int j = 0; j < nCol; j++)
            values[i][j] = gmatrix.values[i][j] - gmatrix1.values[i][j];
}

double GMatrix::trace() const {
    int j = (nRow < nCol) ? nRow : nCol;
    double d = 0.0;

    for (int i = 0; i < j; i++)
        d += values[i][i];

    return d;
}

void GMatrix::transpose() {
    if (nRow != nCol) {
        Math::swap(nRow, nCol);
        DoubleArray2 ad(nRow, nCol);

        for (int i = 0; i < nRow; i++)
            for (int j = 0; j < nCol; j++)
                ad[i][j] = values[j][i];
        values.swap(ad);
    } else {
        for (int i = 0; i < nRow; i++) {
            for (int j = 0; j < i; j++) {
                Math::swap(values[i][j], values[j][i]);
            }
        }
    }
}

void GMatrix::transpose(const GMatrix& gmatrix) {
    if (&gmatrix != this) {
        for (int i = 0; i < nRow; i++)
            for (int j = 0; j < nCol; j++)
                values[i][j] = gmatrix.values[j][i];
    } else
        transpose();
}

// private

void GMatrix::checkMatrix(GMatrix gmatrix) {
    for (int i = 0; i < gmatrix.nRow; i++) {
        for (int j = 0; j < gmatrix.nCol; j++) {
            if (Math::abs(gmatrix.values[i][j]) < EPS)
                cout << " 0.0     ";
            else
                cout << " " << gmatrix.values[i][j];
        }
        cout << endl;
    }
}

void GMatrix::invertGeneral(GMatrix& gmatrix) {
    int size = gmatrix.nRow * gmatrix.nCol;
    double *ad = new double[size];
    double *ad1 = new double[size];
    int *ai = new int[gmatrix.nRow];

    for (int i = 0; i < nRow; i++)
        for (int j = 0; j < nCol; j++)
            ad[i * nCol + j] = gmatrix.values[i][j];

    // identity
    for (int i = 0; i < size; i++)
        ad1[i] = 0.0;

    for (int i = 0; i < nCol; i++)
        ad1[i + i * nCol] = 1.0;

    luBacksubstitution(gmatrix.nRow, ad, ai, ad1);

    for (int i = 0; i < nRow; i++)
        for (int j = 0; j < nCol; j++)
            values[i][j] = ad1[i * nCol + j];

        delete []ad;
        delete []ad1;
        delete []ai;
}


void GMatrix::print_m(const GMatrix& gmatrix, const GMatrix& gmatrix1, const GMatrix& gmatrix2) {
    GMatrix gmatrix3(gmatrix.nCol, gmatrix.nRow);
    //?????????
    gmatrix3.setIdentity();
    // G3 = U * E
    gmatrix3.mul(gmatrix1, gmatrix3);
    // G3 = G3 * V
    gmatrix3.mul(gmatrix3, gmatrix2);
//    cout << endl << "m = " << endl << gmatrix3 << gmatrix1 << gmatrix2 << endl;
}

void GMatrix::print_se(double ad[], double ad1[]) {
    cout << "s=" << ad[0] << " " << ad[1] << " " << ad[2] << endl;
    cout << "e=" << ad1[0] << " " << ad1[1] << endl;
}

void GMatrix::print_svd(const double ad[], const double ad1[], const GMatrix& gmatrix, const GMatrix& gmatrix1) {
    cerr << "GMatrix.print_svd FIXME" << endl;
    //
    //          cout << endl << "s = " << endl;
    //          for (int i = 0; i < ad.length; i++)
    //              cout << " " << ad[i];
    //
    //          cout << endl << "e = " << endl;
    //          for (int j = 0; j < ad1.length; j++)
    //              cout << " " << ad1[j];
    //
//    cout << endl << "u  = " << endl << gmatrix << endl;
//    cout << endl << "v  = " << endl << gmatrix1 << endl;
    GMatrix gmatrix2(gmatrix.nCol, gmatrix1.nRow);
    gmatrix2.setIdentity();
    //          for (int k = 0; k < ad.length; k++)
    //              gmatrix2.values[k][k] = ad[k];
    //
    //          for (int l = 0; l < ad1.length; l++)
    //              gmatrix2.values[l][l + 1] = ad1[l];
    //
//    cout << endl << "m  = " << endl << gmatrix2 << endl;
    gmatrix2.mulTransposeLeft(gmatrix, gmatrix2);
    gmatrix2.mulTransposeRight(gmatrix2, gmatrix1);
//    cout << endl << "u.transpose*m*v.transpose  = " << endl << gmatrix2 << endl;
}

void GMatrix::update_u(int i, GMatrix& gmatrix, double ad[], double ad1[]) {
    for (int j = 0; j < gmatrix.nCol; j++) {
        double d = gmatrix.values[i][j];
        gmatrix.values[i][j] = ad[0] * d + ad1[0] * gmatrix.values[i + 1][j];
        gmatrix.values[i + 1][j] = -ad1[0] * d + ad[0] * gmatrix.values[i + 1][j];
    }
}


void GMatrix::update_u_split(int i, int j, GMatrix& gmatrix, double ad[], double ad1[],
                             bool flag, GMatrix& gmatrix1, GMatrix& gmatrix2) {
    for (int k = 0; k < gmatrix.nCol; k++) {
        double d = gmatrix.values[i][k];
        gmatrix.values[i][k] = ad[0] * d - ad1[0] * gmatrix.values[j][k];
        gmatrix.values[j][k] = ad1[0] * d + ad[0] * gmatrix.values[j][k];
    }

    if (flag) {
        gmatrix1.setIdentity();
        for (int l = 0; l < gmatrix.nCol; l++) {
            double d1 = gmatrix1.values[i][l];

            gmatrix1.values[i][l] = ad[0] * d1 - ad1[0] * gmatrix1.values[j][l];
            gmatrix1.values[j][l] = ad1[0] * d1 + ad[0] * gmatrix1.values[j][l];
        }
    }
    cout << endl << "m=" << endl;
    checkMatrix(gmatrix2);
    cout << endl << "u=" << endl;
    checkMatrix(gmatrix1);
    gmatrix2.mul(gmatrix1, gmatrix2);
    cout << endl << "t*m=" << endl;
    checkMatrix(gmatrix2);
}

void GMatrix::update_v(int i, GMatrix& gmatrix, double ad[], double ad1[]) {
    for (int j = 0; j < gmatrix.nRow; j++) {
        double d = gmatrix.values[j][i];
        gmatrix.values[j][i] = ad[0] * d + ad1[0] * gmatrix.values[j][i + 1];
        gmatrix.values[j][i + 1] = -ad1[0] * d + ad[0] * gmatrix.values[j][i + 1];
    }
}

void GMatrix::update_v_split(int i, int j, GMatrix& gmatrix, double ad[], double ad1[],
                             bool flag, GMatrix& gmatrix1, GMatrix& gmatrix2) {
    for (int k = 0; k < gmatrix.nRow; k++) {
        double d = gmatrix.values[k][i];

        gmatrix.values[k][i] = ad[0] * d - ad1[0] * gmatrix.values[k][j];
        gmatrix.values[k][j] = ad1[0] * d + ad[0] * gmatrix.values[k][j];
    }

    if (flag) {
        gmatrix1.setIdentity();
        for (int l = 0; l < gmatrix.nRow; l++) {
            double d1 = gmatrix1.values[l][i];

            gmatrix1.values[l][i] = ad[0] * d1 - ad1[0] * gmatrix1.values[l][j];
            gmatrix1.values[l][j] = ad1[0] * d1 + ad[0] * gmatrix1.values[l][j];
        }

    }
    cout << "topr   =" << i << endl;
    cout << "bottomr=" << j << endl;
    cout << "cosr=" << ad[0] << endl;
    cout << "sinr=" << ad1[0] << endl;
    cout << endl << "m=" << endl;
    checkMatrix(gmatrix2);
    cout << endl << "v=" << endl;
    checkMatrix(gmatrix1);
    gmatrix2.mul(gmatrix2, gmatrix1);
    cout << endl << "t*m=" << endl;
    checkMatrix(gmatrix2);
}

void GMatrix::chase_across(double ad[], double ad1[], int i, GMatrix& gmatrix,
                           bool flag) {
    double ad2[1];
    double ad3[1];
    GMatrix gmatrix1(gmatrix.nRow, gmatrix.nCol);
    GMatrix gmatrix2(gmatrix.nRow, gmatrix.nCol);

    cerr << "GMatrix.chase_across FIXME" << endl;
    if (flag) {
        gmatrix2.setIdentity();
        //          for (int j = 0; j < ad.length; j++)
        //              gmatrix2.values[j][j] = ad[j];
        //
        //          for (int k = 0; k < ad1.length; k++)
        //              gmatrix2.values[k][k + 1] = ad1[k];
    }

    double d1 = ad1[i];
    double d = ad[i + 1];
    int l;

    for (l = i; l < gmatrix.nCol - 2; l++) {
        double d2 = compute_rot(d, d1, ad3, ad2);

        d1 = -ad1[l + 1] * ad3[0];
        d = ad[l + 2];
        ad[l + 1] = d2;
        ad1[l + 1] = ad1[l + 1] * ad2[0];
        update_u_split(i, l + 1, gmatrix, ad2, ad3, flag, gmatrix1, gmatrix2);
    }

    ad[l + 1] = compute_rot(d, d1, ad3, ad2);
    update_u_split(i, l + 1, gmatrix, ad2, ad3, flag, gmatrix1, gmatrix2);
}

void GMatrix::chase_up(double ad[], double ad1[], int i, GMatrix& gmatrix, bool flag) {
    double ad2[1];
    double ad3[1];
    GMatrix gmatrix1(gmatrix.nRow, gmatrix.nCol);
    GMatrix gmatrix2(gmatrix.nRow, gmatrix.nCol);

    cerr << "GMatrix.chase_up FIXME" << endl;
    if (flag) {
        gmatrix2.setIdentity();
        //          for (int j = 0; j < ad.length; j++)
        //              gmatrix2.values[j][j] = ad[j];
        //
        //          for (int k = 0; k < ad1.length; k++)
        //              gmatrix2.values[k][k + 1] = ad1[k];
    }

    double d = ad1[i];
    double d1 = ad[i];
    int l;

    for (l = i; l > 0; l--) {
        double d2 = compute_rot(d, d1, ad3, ad2);

        d = -ad1[l - 1] * ad3[0];
        d1 = ad[l - 1];
        ad[l] = d2;
        ad1[l - 1] = ad1[l - 1] * ad2[0];
        update_v_split(l, i + 1, gmatrix, ad2, ad3, flag, gmatrix1, gmatrix2);
    }

    ad[l + 1] = compute_rot(d, d1, ad3, ad2);
    update_v_split(l, i + 1, gmatrix, ad2, ad3, flag, gmatrix1, gmatrix2);
}

ostream& operator <<(ostream& os, const GMatrix& m) {
    for (int i = 0; i < m.getNumRow(); i++) {
        for (int j = 0; j < m.getNumCol(); j++)
            os << " " << m.values[i][j];
        os << endl;
    }
    return os;
}

---