// File: /workspace/github_cpp_makefile_star2_O3/DavidLBruce[P]sim3dof/Matrix/build/../../Matrix/src/Matrix.cpp
// Function: Matrix::Inv()
Matrix Matrix::Inv()
{
  //
  // Check to make sure the matrix is square.
  //
  if (rows != cols)
  {
    throw(DIMERR());
  }

  int    col(0);
  double factor(0.0);        // xmult
  int    identElem(0);       //
  int    pivotRow(0);        // lSubk
  int    pivotRowTrial(0);   //
  double *pRhs(NULL);        //
  double *pRowMaxes(NULL);   // sSubi
  int    *pRowOrder(NULL);   // l
  double ratio(0.0);         // r
  double ratioMax(0.0);      // rmax
  int    row(0);             // i1
  double rowMax(0.0);        //
  double sum(0.0);           //
  int    temp(0);            //
  Matrix result(rows, cols); //
  int    rowIdx;             //
  int    rowRatioMax;        // 
  int    rowMaxIdx;          //
  int    scaleRow;           // lSubi
  int    zeroColIdx(0);      // k

  #ifdef debugEn
  FILE*fp = fopen("Debug.txt", "w");
  #endif
  //
  // Forward elimination with scaled pivoting.  This algorithm calculates an
  // upper triangular matrix of coefficients called upTriang.  The resulting
  // system is:
  //
  //   upTriang * x = b'
  //
  // The upper triangular system is easily solved for x by back substitution,
  // starting at the last row.
  //
  // The solution to the upper triangular system x also solves the original 
  // linear system:
  //
  //   Ax = b
  //
  // Later, sucessive columns of the identity matrix will be substituted for
  // b, resulting in successive columns of the inv(A) matrix.
  //
  //   A*inv(A) = I
  //
  // Gaussian elimination takes one equation of a system of linear equations
  // as a pivot row.  For each of the remaining rows a scaled version of the 
  // pivot row is subtracted, such that one element of the selected row is 
  // zeroed.
  //
  // Then a new pivot row is selected and the process is repeated eliminating
  // a new element from the remaining rows.
  //
  // In order to reduce the chance for errors due to finite arithmetic, the
  // sequence of pivot rows is selected such that the initial scale ratio is
  // largest.  Successive pivot rows are selected in order of decreasing 
  // scale ratio.  Rather than reorder the system of equitions, a vector is 
  // created to maintain the order in which the pivot rows are selected. 
  // The order in which the pivot rows are selected is stored in the rowOrder
  // vector.
  //
  pRowOrder = new int[rows];

  //
  // The abs(maximum) of each row is stored in pRowMaxws vector.
  //
  pRowMaxes = new double[rows];

  //
  // Create a copy of the matrix.  The algorithm will convert the new matrix
  // into upper triangular form as it proceeds.
  //
  Matrix upTriang(*this);

  //
  // Find the largest coefficient in each row.
  //
  for (row = 0; row < rows; ++row)
  {
    //
    // Initialize the pivot row order as the initial matrix row order.  This
    // order will be modified as the algorithm progresses.
    //
    pRowOrder[row] = row;

    rowMax = 0.0;
    for (col = 0; col < cols; ++col)
    {
      rowMax = Max(rowMax, fabs(upTriang[row][col]));
    }
    pRowMaxes[row] = rowMax;
  }

  #ifdef debugEn
  fprintf(fp, "p05 pRowMaxes = %7.2f %7.2f %7.2f %7.2f \n", 
               pRowMaxes[0], pRowMaxes[1], pRowMaxes[2], pRowMaxes[3]);
  fprintf(fp, "pRowOrder = %d %d %d %d \n", pRowOrder[0], pRowOrder[1],
                                    pRowOrder[2], pRowOrder[3]);
  #endif

  //
  // Step through the system along the diagonal, selecting the appropriate pivot
  // element for each column.
  //
  for (zeroColIdx = 0; zeroColIdx < cols - 1; ++zeroColIdx)
  {
    #ifdef debugEn
    fprintf(fp, "p10 zeroColIdx (k) = %d \n", zeroColIdx);
    #endif

    //
    // Find the row with the largest scale ratio for each column.  This will be
    // the pivot row for that column.
    //
    ratioMax = 0.0;

    for (rowIdx = zeroColIdx; rowIdx < rows; ++rowIdx)
    {
      //
      // See if the next row should be the pivot row for this pivot index by
      // selecting the highest scale ratio.
      //
      pivotRowTrial = pRowOrder[rowIdx];

      //
      // The denomiator in the ratio calculation below should never be zero
      // since it was chosen to be the max relative to the row.
      //
      if (pRowMaxes[pivotRowTrial] == 0.0)
      {
        throw(SINGERR());
      }

      ratio = fabs(upTriang[pivotRowTrial][zeroColIdx] / 
                                                  pRowMaxes[pivotRowTrial]);

      if (ratio > ratioMax)
      {
        ratioMax = ratio;

        rowRatioMax = rowIdx;
      }
    } // end for (rowIdx = zeroColIdx; rowIdx < rows; ++rowIdx)

    #ifdef debugEn
    fprintf(fp, "p20 ratioMax = %7.2f \n", ratioMax);
    fprintf(fp, "p30 rowRatioMax (j) = %d \n", rowRatioMax); 
    #endif

    //
    // Sort the pivot row list by interchanging the newly found pivot row with
    // the current row pointed to by the pivot index.
    //
    temp = pRowOrder[zeroColIdx];
    pRowOrder[zeroColIdx] = pRowOrder[rowRatioMax];
    pRowOrder[rowRatioMax] = temp;
    
    #ifdef debugEn
    fprintf(fp, "p40: lSubj <--> lSubk \n");
    fprintf(fp, "p50: pRowOrder = %d %d %d %d \n", pRowOrder[0], pRowOrder[1],
                                    pRowOrder[2], pRowOrder[3]);
    #endif

    pivotRow = pRowOrder[zeroColIdx];
    
    #ifdef debugEn
    fprintf(fp, "p30 pivotRow (j) = %d \n", pivotRow); 
    #endif

    for (rowIdx = zeroColIdx + 1; rowIdx < rows; ++rowIdx)
    {
      scaleRow = pRowOrder[rowIdx];
      
      #ifdef debugEn
      fprintf(fp, "p70 scaleRow = row being scaled %d \n", scaleRow);
      #endif
  
      //
      // The denomenator in the factor calculation should never be zero since it
      // was chosen to be the max relative to the row max.
      //
      if (upTriang[pivotRow][zeroColIdx] == 0.0)
      {
        throw(SINGERR());
      }
      factor = upTriang[scaleRow][zeroColIdx] / upTriang[pivotRow][zeroColIdx];
    
      //
      // Store the scaling factor in the upper triangular matrix for use later.
      // This makes the book keeping easy since it can be accessed using the 
      // row and zeroColIdx indeces.  Of course the matrix is no linger strictly 
      // upper traingular.
      //
      upTriang[scaleRow][zeroColIdx] = factor;
    
      //
      // Zero the elements in the pivot column in the remaining rows to create
      // the upper triangular matrix.
      //
      for (col = zeroColIdx + 1; col < cols; ++col)
      {
        upTriang[scaleRow][col] -= factor * upTriang[pivotRow][col];
    
      }

      #ifdef debugEn
      fprintf(fp, "p80 row = %d; factor = %7.2f;", scaleRow, factor);
      fprintf(fp, " upTriang(%d,:) = %7.2f %7.2f %7.2f %7.2f \n", row,
                  upTriang[scaleRow][0], upTriang[scaleRow][1], 
                  upTriang[scaleRow][2], upTriang[scaleRow][3]);
      #endif
    
    } // end for (rowIdx = zeroColIdx + 1; rowIdx < rows; ++rowIdx)
  }  // for (zeroColIdx = 0; zeroColIdx < cols - 1; ++zeroColIdx)

  #ifdef debugEn
  fprintf(fp, "p90 upTriang = \n");
  for (int dbRow = 0; dbRow < 4; ++dbRow)
  {
    for (int dbCol = 0; dbCol < 4; ++dbCol)
    {
      fprintf(fp, "%7.2f ", upTriang[dbRow][dbCol]); 
    }
    fprintf(fp, "\n");
  }
  #endif

  //
  // We now have an upper triangular representation of A in 
  //  A * X = b
  // But we have not operated on b.  Furthermore, what we want is inv(A) as in
  //  A * inv(A) = I
  // We'll brek this problem up into columns of inv(A) and I
  //  A * inv(A)(i) = I(i)
  // So we need to apply forward elimination processing to the columns of I.
  // Then proceed to back substitution
  //
  // The modified right hand side system of equations is stored in pRhs.
  //
  //   A * X = b (rhs)
  //
  pRhs = new double[rows];

  for (identElem = 0; identElem < rows; ++identElem)
  {
    //
    // Extract the appropriate column of the identify matrix for use as 
    // the right hand side of the system of equations.
    //
    for (row = 0; row < rows; ++row)
    {
      pRhs[row] = 0.0;
    }
    pRhs[identElem] = 1.0;

    //
    // Apply the same scaling used on the left hand side of the system
    // to the right hand side.
    //
    for (zeroColIdx = 0; zeroColIdx < rows-1; ++zeroColIdx)
    {
      pivotRow = pRowOrder[zeroColIdx];
      
      for (rowIdx = zeroColIdx + 1; rowIdx < rows; ++rowIdx)
      {
        row = pRowOrder[rowIdx];

        factor = upTriang[row][zeroColIdx];

        pRhs[row] -= factor * pRhs[pivotRow];
      }
    }

    //
    // Back substitution starting a the bottome of the triangular matrix.
    //
    row = pRowOrder[rows - 1];

    if (upTriang[row][rows-1] == 0.0)
    {
      throw(SINGERR());
    }

    result[rows-1][identElem] = pRhs[row] / upTriang[row][rows-1];

    for (rowIdx = rows-2; rowIdx >= 0; --rowIdx)
    {
      row = pRowOrder[rowIdx];
      sum = pRhs[row];
      
      for (col = rowIdx + 1; col < cols; ++col)
      {
        sum = sum - upTriang[row][col] * result[col][identElem];
      }
     
      if (upTriang[row][rowIdx] == 0.0)
      {
      throw(SINGERR());
      }
      result[rowIdx][identElem] = sum / upTriang[row][rowIdx];
    }
  }  // end for (identElem = 0; identElem < rows; ++identElem)

  delete [] pRhs;
  delete [] pRowOrder;
  delete [] pRowMaxes;

  #ifdef debugEn
  fclose(fp);
  #endif

  return(result);
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x78, %rsp
movl 0x8(%rsi), %edx
cmpl (%rsi), %edx
jne 0x147fe
movq %rsi, %rbx
xorpd %xmm0, %xmm0
movq %rdi, -0x48(%rbp)
movl %edx, %esi
callq 0x13382
movq %rbx, -0x30(%rbp)
movslq 0x8(%rbx), %rbx
leaq (,%rbx,4), %rdi
testq %rbx, %rbx
movq $-0x1, %r14
cmovsq %r14, %rdi
callq 0x3030
movq %rax, %r12
movq %rbx, %rdi
shlq $0x3, %rdi
testl %ebx, %ebx
cmovsq %r14, %rdi
callq 0x3030
movq %rax, -0x78(%rbp)
leaq -0x90(%rbp), %rdi
movq -0x30(%rbp), %rbx
movq %rbx, %rsi
callq 0x133e2
movl (%rbx), %eax
movl 0x8(%rbx), %r14d
testl %r14d, %r14d
jle 0x142fb
xorl %r15d, %r15d
leaq -0x90(%rbp), %rbx
movl %r15d, (%r12,%r15,4)
xorpd %xmm0, %xmm0
testl %eax, %eax
jle 0x142e6
movsd %xmm0, -0x38(%rbp)
xorl %r14d, %r14d
movq %rbx, %rdi
movl %r15d, %esi
callq 0x139a6
movsd (%rax,%r14,8), %xmm0
andpd 0x22db(%rip), %xmm0     # 0x165a0
maxsd -0x38(%rbp), %xmm0
incq %r14
movq -0x30(%rbp), %rax
movslq (%rax), %rax
movsd %xmm0, -0x38(%rbp)
cmpq %rax, %r14
jl 0x142ac
movq -0x30(%rbp), %rcx
movl 0x8(%rcx), %r14d
movq -0x78(%rbp), %rcx
movsd %xmm0, (%rcx,%r15,8)
incq %r15
movslq %r14d, %rcx
cmpq %rcx, %r15
jl 0x14298
cmpl $0x2, %eax
movq %r12, -0x50(%rbp)
jl 0x144f9
movl $0x1, %eax
movq %rax, -0x70(%rbp)
xorl %r9d, %r9d
leaq -0x90(%rbp), %r12
movq -0x30(%rbp), %rdi
movslq %r14d, %rax
cmpq %rax, %r9
movq %r9, -0x40(%rbp)
jge 0x143b2
xorpd %xmm0, %xmm0
movsd %xmm0, -0x38(%rbp)
movq %r9, %rbx
movq -0x50(%rbp), %rax
movslq (%rax,%rbx,4), %rsi
movq -0x78(%rbp), %rax
movsd (%rax,%rsi,8), %xmm0
movsd %xmm0, -0x60(%rbp)
ucomisd 0x226f(%rip), %xmm0     # 0x165c8
jne 0x14361
jnp 0x147d4
movq %r12, %rdi
callq 0x139a6
movq -0x40(%rbp), %r9
movsd (%rax,%r9,8), %xmm0
divsd -0x60(%rbp), %xmm0
andpd 0x2220(%rip), %xmm0     # 0x165a0
movsd -0x38(%rbp), %xmm1
ucomisd %xmm1, %xmm0
maxsd %xmm1, %xmm0
movl -0x68(%rbp), %eax
cmoval %ebx, %eax
movl %eax, -0x68(%rbp)
incq %rbx
movq -0x30(%rbp), %rax
movl 0x8(%rax), %r14d
movsd %xmm0, -0x38(%rbp)
cmpl %ebx, %r14d
jg 0x1433b
movslq %r14d, %rax
movq -0x30(%rbp), %rdi
movq -0x50(%rbp), %r8
movl (%r8,%r9,4), %ecx
movslq -0x68(%rbp), %rdx
movl (%r8,%rdx,4), %esi
movl %esi, (%r8,%r9,4)
movl %ecx, (%r8,%rdx,4)
leaq 0x1(%r9), %rcx
movq %rcx, -0x98(%rbp)
cmpq %rax, %rcx
jge 0x144db
movl (%r8,%r9,4), %r13d
movq -0x70(%rbp), %rbx
movq -0x50(%rbp), %rax
movl (%rax,%rbx,4), %r15d
movq %r12, %rdi
movl %r13d, %esi
callq 0x139a6
movq -0x40(%rbp), %rcx
movsd (%rax,%rcx,8), %xmm0
ucomisd 0x21be(%rip), %xmm0     # 0x165c8
jne 0x14412
jnp 0x147d4
movq %r12, %rdi
movl %r15d, %esi
callq 0x139a6
movq -0x40(%rbp), %r14
movsd (%rax,%r14,8), %xmm0
movapd %xmm0, -0x60(%rbp)
movq %r12, %rdi
movl %r13d, %esi
callq 0x139a6
movapd -0x60(%rbp), %xmm0
divsd (%rax,%r14,8), %xmm0
movapd %xmm0, -0x60(%rbp)
movq %r12, %rdi
movl %r15d, %esi
callq 0x139a6
movapd -0x60(%rbp), %xmm0
movsd %xmm0, (%rax,%r14,8)
movq -0x30(%rbp), %rax
movslq (%rax), %rax
cmpq %rax, -0x98(%rbp)
jge 0x144c5
movaps -0x60(%rbp), %xmm0
xorps 0x1b98(%rip), %xmm0     # 0x16010
movaps %xmm0, -0x60(%rbp)
movq -0x70(%rbp), %r14
movq %r12, %rdi
movl %r13d, %esi
callq 0x139a6
movsd (%rax,%r14,8), %xmm0
movsd %xmm0, -0x38(%rbp)
movq %r12, %rdi
movl %r15d, %esi
callq 0x139a6
movsd -0x38(%rbp), %xmm0
mulsd -0x60(%rbp), %xmm0
addsd (%rax,%r14,8), %xmm0
movsd %xmm0, (%rax,%r14,8)
incq %r14
movq -0x30(%rbp), %rax
movl (%rax), %eax
cmpl %r14d, %eax
jg 0x14480
incq %rbx
movq -0x30(%rbp), %rcx
movl 0x8(%rcx), %r14d
cmpl %ebx, %r14d
jg 0x143e6
jmp 0x144dd
movl (%rdi), %eax
decl %eax
cltq
incq -0x70(%rbp)
movq -0x98(%rbp), %r9
cmpq %rax, %r9
movq -0x30(%rbp), %rdi
jl 0x1431f
movslq %r14d, %rax
leaq (,%rax,8), %rcx
testl %eax, %eax
movq $-0x1, %rdi
cmovnsq %rcx, %rdi
callq 0x3030
movq %rax, -0x40(%rbp)
movq -0x50(%rbp), %rbx
testl %r14d, %r14d
jle 0x14799
xorl %r13d, %r13d
testl %r14d, %r14d
movq %r13, -0x68(%rbp)
jle 0x145e9
movl %r14d, %edx
shlq $0x3, %rdx
movq -0x40(%rbp), %rbx
movq %rbx, %rdi
xorl %esi, %esi
callq 0x30e0
movabsq $0x3ff0000000000000, %rax # imm = 0x3FF0000000000000
movq %rax, (%rbx,%r13,8)
cmpl $0x1, %r14d
je 0x145fb
movl $0x1, %eax
movq %rax, -0x60(%rbp)
xorl %r12d, %r12d
leaq 0x1(%r12), %rcx
movslq %r14d, %rax
movq %rcx, -0x38(%rbp)
cmpq %rax, %rcx
jge 0x145d4
movq -0x50(%rbp), %rax
movslq (%rax,%r12,4), %r15
movq -0x60(%rbp), %r13
movq -0x50(%rbp), %rax
movl (%rax,%r13,4), %ebx
leaq -0x90(%rbp), %rdi
movl %ebx, %esi
callq 0x139a6
movslq %ebx, %rcx
movsd (%rax,%r12,8), %xmm0
movq -0x40(%rbp), %rax
movsd (%rax,%rcx,8), %xmm1
mulsd (%rax,%r15,8), %xmm0
subsd %xmm0, %xmm1
movsd %xmm1, (%rax,%rcx,8)
incq %r13
movq -0x30(%rbp), %rax
movl 0x8(%rax), %r14d
cmpl %r13d, %r14d
jg 0x1458d
leal -0x1(%r14), %eax
cltq
incq -0x60(%rbp)
movq -0x38(%rbp), %r12
cmpq %rax, %r12
jl 0x14570
jmp 0x14601
movq -0x40(%rbp), %rax
movabsq $0x3ff0000000000000, %rcx # imm = 0x3FF0000000000000
movq %rcx, (%rax,%r13,8)
decl %r14d
movslq %r14d, %rax
movq -0x50(%rbp), %rcx
movl (%rcx,%rax,4), %ebx
leaq -0x90(%rbp), %rdi
movl %ebx, %esi
callq 0x139a6
movq -0x68(%rbp), %r13
movq -0x30(%rbp), %rcx
movslq 0x8(%rcx), %rcx
movsd -0x8(%rax,%rcx,8), %xmm0
ucomisd 0x1f98(%rip), %xmm0     # 0x165c8
jne 0x14638
jnp 0x147d4
movslq %ebx, %rax
movq -0x40(%rbp), %rcx
movsd (%rcx,%rax,8), %xmm0
movsd %xmm0, -0x38(%rbp)
leaq -0x90(%rbp), %rdi
movl %ebx, %esi
callq 0x139a6
movq -0x30(%rbp), %rcx
movslq 0x8(%rcx), %rsi
movsd -0x8(%rax,%rsi,8), %xmm0
movsd %xmm0, -0x60(%rbp)
decq %rsi
movq -0x48(%rbp), %rdi
callq 0x139a6
movsd -0x38(%rbp), %xmm0
divsd -0x60(%rbp), %xmm0
movsd %xmm0, (%rax,%r13,8)
movq -0x30(%rbp), %rax
movl 0x8(%rax), %r14d
cmpl $0x2, %r14d
movq -0x50(%rbp), %rbx
jl 0x1478a
leal -0x2(%r14), %r15d
decl %r14d
movslq (%rbx,%r15,4), %rbx
movq -0x40(%rbp), %rax
movsd (%rax,%rbx,8), %xmm0
movsd %xmm0, -0x38(%rbp)
leaq 0x1(%r15), %rax
movq -0x30(%rbp), %rcx
movslq (%rcx), %rcx
cmpq %rcx, %rax
jge 0x14712
movq %r14, %r12
leaq -0x90(%rbp), %rdi
movl %ebx, %esi
callq 0x139a6
movsd (%rax,%r12,8), %xmm0
movsd %xmm0, -0x60(%rbp)
movq -0x48(%rbp), %rdi
movl %r12d, %esi
callq 0x139a6
movsd -0x60(%rbp), %xmm1
mulsd (%rax,%r13,8), %xmm1
movsd -0x38(%rbp), %xmm0
subsd %xmm1, %xmm0
movsd %xmm0, -0x38(%rbp)
incq %r12
movq -0x30(%rbp), %rax
cmpl %r12d, (%rax)
jg 0x146c8
leaq -0x90(%rbp), %rdi
movl %ebx, %esi
callq 0x139a6
movsd (%rax,%r15,8), %xmm0
ucomisd 0x1e9a(%rip), %xmm0     # 0x165c8
jne 0x14736
jnp 0x147d4
leaq -0x90(%rbp), %rdi
movl %ebx, %esi
callq 0x139a6
movsd (%rax,%r15,8), %xmm0
movsd %xmm0, -0x60(%rbp)
movq -0x48(%rbp), %rdi
movl %r15d, %esi
callq 0x139a6
movsd -0x38(%rbp), %xmm0
divsd -0x60(%rbp), %xmm0
movsd %xmm0, (%rax,%r13,8)
leaq -0x1(%r15), %rax
decq %r14
testq %r15, %r15
movq %rax, %r15
movq -0x50(%rbp), %rbx
jg 0x146a3
movq -0x30(%rbp), %rax
movl 0x8(%rax), %r14d
incq %r13
movslq %r14d, %rax
cmpq %rax, %r13
jl 0x1452a
movq -0x40(%rbp), %rdi
callq 0x3200
movq %rbx, %rdi
callq 0x3200
movq -0x78(%rbp), %rdi
callq 0x3200
movq -0x80(%rbp), %rdi
testq %rdi, %rdi
je 0x147c1
callq 0x3200
movq -0x48(%rbp), %rax
addq $0x78, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movl $0x8, %edi
callq 0x30b0
leaq 0x95b3(%rip), %rcx      # 0x1dd98
movq %rcx, (%rax)
leaq 0x9531(%rip), %rsi      # 0x1dd20
movq 0x97ea(%rip), %rdx      # 0x1dfe0
movq %rax, %rdi
callq 0x32b0
movl $0x8, %edi
callq 0x30b0
leaq 0x9539(%rip), %rcx      # 0x1dd48
movq %rcx, (%rax)
leaq 0x94d7(%rip), %rsi      # 0x1dcf0
movq 0x97c0(%rip), %rdx      # 0x1dfe0
movq %rax, %rdi
callq 0x32b0
jmp 0x1482a
movq %rax, %rbx
jmp 0x14850
jmp 0x1483f
jmp 0x1483f
jmp 0x1483f
jmp 0x1483f
jmp 0x1483f
jmp 0x1483f
jmp 0x1483f
jmp 0x1483f
movq %rax, %rbx
movq -0x80(%rbp), %rdi
testq %rdi, %rdi
je 0x14850
callq 0x3200
movq -0x48(%rbp), %rax
movq 0x10(%rax), %rdi
testq %rdi, %rdi
je 0x14866
callq 0x3200
movq -0x48(%rbp), %rax
movq $0x0, 0x10(%rax)
movq %rbx, %rdi
callq 0x32d0