// File: /workspace/github_cpp_makefile_star2_O1/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
movq %rsi, %rbx
movl (%rsi), %edx
movl 0x8(%rsi), %esi
cmpl %edx, %esi
jne 0x14972
xorpd %xmm0, %xmm0
movq %rdi, -0x58(%rbp)
callq 0x1350e
movq %rbx, -0x60(%rbp)
movslq 0x8(%rbx), %rbx
leaq (,%rbx,4), %rdi
testq %rbx, %rbx
movq $-0x1, %r14
cmovsq %r14, %rdi
callq 0x3030
movq %rax, -0x40(%rbp)
movq %rbx, %rdi
shlq $0x3, %rdi
testl %ebx, %ebx
cmovsq %r14, %rdi
callq 0x3030
movq %rax, %r12
leaq -0x90(%rbp), %rdi
movq -0x60(%rbp), %r14
movq %r14, %rsi
callq 0x13576
cmpl $0x0, 0x8(%r14)
jle 0x144a7
xorl %ebx, %ebx
leaq -0x90(%rbp), %r15
movq -0x40(%rbp), %rax
movl %ebx, (%rax,%rbx,4)
cmpl $0x0, (%r14)
jle 0x1448d
xorpd %xmm0, %xmm0
movsd %xmm0, -0x30(%rbp)
xorl %r14d, %r14d
movq %r15, %rdi
movl %ebx, %esi
callq 0x13b48
movsd (%rax,%r14,8), %xmm0
andpd 0x212e(%rip), %xmm0     # 0x165a0
maxsd -0x30(%rbp), %xmm0
incq %r14
movq -0x60(%rbp), %rax
movslq (%rax), %rax
movsd %xmm0, -0x30(%rbp)
cmpq %rax, %r14
jl 0x1445a
jmp 0x14491
xorpd %xmm0, %xmm0
movsd %xmm0, (%r12,%rbx,8)
incq %rbx
movq -0x60(%rbp), %r14
movslq 0x8(%r14), %rax
cmpq %rax, %rbx
jl 0x14441
movq %r12, -0x68(%rbp)
cmpl $0x2, (%r14)
jl 0x14687
movl $0x1, %eax
movq %rax, -0x78(%rbp)
xorl %edi, %edi
leaq -0x90(%rbp), %r12
movslq 0x8(%r14), %rax
cmpq %rax, %rdi
movq %rdi, -0x38(%rbp)
jge 0x14545
xorpd %xmm0, %xmm0
movq %rdi, %r15
movq -0x68(%rbp), %rcx
movsd %xmm0, -0x30(%rbp)
movq -0x40(%rbp), %rax
movslq (%rax,%r15,4), %rbx
movsd (%rcx,%rbx,8), %xmm0
ucomisd 0x20cf(%rip), %xmm0     # 0x165c8
jne 0x14501
jnp 0x14948
movq %r12, %rdi
movl %ebx, %esi
callq 0x13b48
movq -0x38(%rbp), %rdi
movsd (%rax,%rdi,8), %xmm0
movq -0x68(%rbp), %rcx
divsd (%rcx,%rbx,8), %xmm0
andpd 0x207b(%rip), %xmm0     # 0x165a0
movsd -0x30(%rbp), %xmm1
ucomisd %xmm1, %xmm0
maxsd %xmm1, %xmm0
movl -0x70(%rbp), %eax
cmoval %r15d, %eax
movl %eax, -0x70(%rbp)
incq %r15
cmpl %r15d, 0x8(%r14)
jg 0x144df
movq -0x40(%rbp), %rsi
movl (%rsi,%rdi,4), %eax
movslq -0x70(%rbp), %rcx
movl (%rsi,%rcx,4), %edx
movl %edx, (%rsi,%rdi,4)
movl %eax, (%rsi,%rcx,4)
leaq 0x1(%rdi), %rcx
movslq 0x8(%r14), %rax
movq %rcx, -0x98(%rbp)
cmpq %rax, %rcx
jge 0x1466d
movq -0x40(%rbp), %rax
movq -0x38(%rbp), %rcx
movl (%rax,%rcx,4), %ebx
movq -0x78(%rbp), %rcx
movq -0x40(%rbp), %rax
movq %rcx, -0xa0(%rbp)
movl (%rax,%rcx,4), %r13d
movq %r12, %rdi
movl %ebx, %esi
callq 0x13b48
movq -0x38(%rbp), %rcx
movsd (%rax,%rcx,8), %xmm0
ucomisd 0x201e(%rip), %xmm0     # 0x165c8
jne 0x145b2
jnp 0x14948
movq %r12, %rdi
movl %r13d, %esi
callq 0x13b48
movq -0x38(%rbp), %r15
movsd (%rax,%r15,8), %xmm0
movapd %xmm0, -0x50(%rbp)
movq %r12, %rdi
movl %ebx, %esi
callq 0x13b48
movapd -0x50(%rbp), %xmm0
divsd (%rax,%r15,8), %xmm0
movapd %xmm0, -0x50(%rbp)
movq %r12, %rdi
movl %r13d, %esi
callq 0x13b48
movapd -0x50(%rbp), %xmm0
movsd %xmm0, (%rax,%r15,8)
movslq (%r14), %rax
cmpq %rax, -0x98(%rbp)
jge 0x14659
movaps -0x50(%rbp), %xmm0
xorps 0x19fd(%rip), %xmm0     # 0x16010
movaps %xmm0, -0x50(%rbp)
movq -0x78(%rbp), %r15
movq %r12, %rdi
movl %ebx, %esi
callq 0x13b48
movsd (%rax,%r15,8), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r12, %rdi
movl %r13d, %esi
callq 0x13b48
movsd -0x30(%rbp), %xmm0
mulsd -0x50(%rbp), %xmm0
addsd (%rax,%r15,8), %xmm0
movsd %xmm0, (%rax,%r15,8)
incq %r15
cmpl %r15d, (%r14)
jg 0x1461b
movq -0xa0(%rbp), %rcx
incq %rcx
cmpl %ecx, 0x8(%r14)
jg 0x14580
movslq (%r14), %rax
decq %rax
incq -0x78(%rbp)
movq -0x98(%rbp), %rdi
cmpq %rax, %rdi
jl 0x144c7
movslq 0x8(%r14), %rax
leaq (,%rax,8), %rcx
testq %rax, %rax
movq $-0x1, %rdi
cmovnsq %rcx, %rdi
callq 0x3030
movq %rax, %r13
movl 0x8(%r14), %eax
testl %eax, %eax
movq -0x68(%rbp), %rbx
jle 0x1490e
movq $0x0, -0x38(%rbp)
testl %eax, %eax
jle 0x146d5
movl %eax, %edx
shlq $0x3, %rdx
movq %r13, %rdi
xorl %esi, %esi
callq 0x30e0
movq -0x38(%rbp), %rax
movabsq $0x3ff0000000000000, %rcx # imm = 0x3FF0000000000000
movq %rcx, (%r13,%rax,8)
movl 0x8(%r14), %eax
cmpl $0x2, %eax
jl 0x1477e
movl $0x1, %eax
movq %rax, -0x50(%rbp)
xorl %r12d, %r12d
leaq 0x1(%r12), %rcx
movslq 0x8(%r14), %rax
movq %rcx, -0x30(%rbp)
cmpq %rax, %rcx
jge 0x14764
movq -0x40(%rbp), %rax
movslq (%rax,%r12,4), %r14
movq -0x50(%rbp), %r15
movq -0x40(%rbp), %rax
movl (%rax,%r15,4), %ebx
leaq -0x90(%rbp), %rdi
movl %ebx, %esi
callq 0x13b48
movslq %ebx, %rcx
movsd (%rax,%r12,8), %xmm0
movsd (%r13,%rcx,8), %xmm1
mulsd (%r13,%r14,8), %xmm0
subsd %xmm0, %xmm1
movsd %xmm1, (%r13,%rcx,8)
incq %r15
movq -0x60(%rbp), %rax
cmpl %r15d, 0x8(%rax)
jg 0x1471f
movq -0x60(%rbp), %r14
movslq 0x8(%r14), %rax
decq %rax
incq -0x50(%rbp)
movq -0x30(%rbp), %r12
cmpq %rax, %r12
jl 0x14701
jmp 0x14780
decl %eax
cltq
movq -0x40(%rbp), %rcx
movl (%rcx,%rax,4), %ebx
leaq -0x90(%rbp), %rdi
movl %ebx, %esi
callq 0x13b48
movslq 0x8(%r14), %rcx
movsd -0x8(%rax,%rcx,8), %xmm0
ucomisd 0x1e1f(%rip), %xmm0     # 0x165c8
jne 0x147b1
jnp 0x14948
movslq %ebx, %rax
movsd (%r13,%rax,8), %xmm0
movsd %xmm0, -0x30(%rbp)
leaq -0x90(%rbp), %rdi
movl %ebx, %esi
callq 0x13b48
movslq 0x8(%r14), %rsi
movsd -0x8(%rax,%rsi,8), %xmm0
movsd %xmm0, -0x50(%rbp)
decq %rsi
movq -0x58(%rbp), %rdi
callq 0x13b48
movsd -0x30(%rbp), %xmm0
divsd -0x50(%rbp), %xmm0
movq -0x38(%rbp), %rcx
movsd %xmm0, (%rax,%rcx,8)
movl 0x8(%r14), %r12d
cmpl $0x2, %r12d
jl 0x148f2
leal -0x2(%r12), %ebx
decl %r12d
movq -0x40(%rbp), %rax
movslq (%rax,%rbx,4), %r15
movsd (%r13,%r15,8), %xmm0
movsd %xmm0, -0x30(%rbp)
leaq 0x1(%rbx), %rax
movslq (%r14), %rcx
cmpq %rcx, %rax
movq %r12, -0x70(%rbp)
jge 0x14880
leaq -0x90(%rbp), %rdi
movl %r15d, %esi
callq 0x13b48
movsd (%rax,%r12,8), %xmm0
movsd %xmm0, -0x50(%rbp)
movq -0x58(%rbp), %rdi
movl %r12d, %esi
callq 0x13b48
movq -0x38(%rbp), %rcx
movsd -0x50(%rbp), %xmm1
mulsd (%rax,%rcx,8), %xmm1
movsd -0x30(%rbp), %xmm0
subsd %xmm1, %xmm0
movsd %xmm0, -0x30(%rbp)
incq %r12
cmpl %r12d, (%r14)
jg 0x14836
leaq -0x90(%rbp), %rdi
movl %r15d, %esi
callq 0x13b48
movq -0x70(%rbp), %r12
movsd (%rax,%rbx,8), %xmm0
ucomisd 0x1d28(%rip), %xmm0     # 0x165c8
jne 0x148a8
jnp 0x14948
leaq -0x90(%rbp), %rdi
movl %r15d, %esi
callq 0x13b48
movsd (%rax,%rbx,8), %xmm0
movsd %xmm0, -0x50(%rbp)
movq -0x58(%rbp), %rdi
movl %ebx, %esi
callq 0x13b48
movsd -0x30(%rbp), %xmm0
divsd -0x50(%rbp), %xmm0
movq -0x38(%rbp), %rcx
movsd %xmm0, (%rax,%rcx,8)
leaq -0x1(%rbx), %rax
decq %r12
testq %rbx, %rbx
movq %rax, %rbx
jg 0x14812
movq -0x38(%rbp), %rdx
incq %rdx
movslq 0x8(%r14), %rax
movq %rdx, -0x38(%rbp)
cmpq %rax, %rdx
movq -0x68(%rbp), %rbx
jl 0x146c1
movq %r13, %rdi
callq 0x3200
movq -0x40(%rbp), %rdi
callq 0x3200
movq %rbx, %rdi
callq 0x3200
movq -0x80(%rbp), %rdi
testq %rdi, %rdi
je 0x14935
callq 0x3200
movq -0x58(%rbp), %rax
addq $0x78, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movl $0x8, %edi
callq 0x30b0
leaq 0x943f(%rip), %rcx      # 0x1dd98
movq %rcx, (%rax)
leaq 0x93bd(%rip), %rsi      # 0x1dd20
movq 0x9676(%rip), %rdx      # 0x1dfe0
movq %rax, %rdi
callq 0x32b0
movl $0x8, %edi
callq 0x30b0
leaq 0x93c5(%rip), %rcx      # 0x1dd48
movq %rcx, (%rax)
leaq 0x9363(%rip), %rsi      # 0x1dcf0
movq 0x964c(%rip), %rdx      # 0x1dfe0
movq %rax, %rdi
callq 0x32b0
jmp 0x1499e
movq %rax, %rbx
jmp 0x149cc
jmp 0x149b3
jmp 0x149b3
jmp 0x149b3
jmp 0x149b3
jmp 0x149b3
jmp 0x149b3
jmp 0x149b3
jmp 0x149b3
movq %rax, %rbx
movq -0x80(%rbp), %rdi
testq %rdi, %rdi
je 0x149c4
callq 0x3200
movq $0x0, -0x80(%rbp)
movq -0x58(%rbp), %rax
movq 0x10(%rax), %rdi
testq %rdi, %rdi
je 0x149de
callq 0x3200
movq -0x58(%rbp), %rax
movq $0x0, 0x10(%rax)
movq %rbx, %rdi
callq 0x32d0