linear_algebrahd_impl.h

很多二维 三维几何计算算法 C++ 类库

  for(i=0; i<rows; i++) { 
    for(j=0; j<cols; j++) 
      C(i,j)=A(i,j); 
    C(i,cols)=b[i]; 
    L(i,i) = 1; // diagonal elements are 1
  }


  std::vector<int> var(cols); 
  // column $j$ of |C| represents the |var[j]| - th variable
  // the array is indexed between |0| and |cols - 1|

  for(j=0; j<cols; j++)
    var[j]= j; // at the beginning, variable $x_j$ stands in column $j$

  RT_ denom = 1; // the determinant of an empty matrix is 1
  int sign = 1; // no interchanges yet
  int rank = 0; // we have not seen any non-zero row yet

  /* here comes the main loop */
  for(k=0; k<rows; k++) {   
    bool non_zero_found = false; 
    for(i = k; i < rows; i++) { // step through rows $k$ to |rows - 1|
      for (j = k ; j < cols && C(i,j) == 0; j++) ; 
      // step through columns |k| to |cols - 1|
      if (j < cols) {  
        non_zero_found = true; 
        break; 
      }
    }


    if ( non_zero_found ) { 
      rank++; //increase the rank
      if (i != k) { 
        sign = -sign; 
        /* we interchange rows |k| and |i| of |L| and |C| */
        L.swap_rows(i,k); C.swap_rows(i,k);
      }
      if (j != k) { 
        /* We interchange columns |k| and |j|, and
           store the exchange of variables in |var| */
        sign = - sign; 
        C.swap_columns(j,k);
        std::swap(var[k],var[j]); 
      }
       
      for(i = k + 1; i < rows; i++)  
        for (j = 0; j <  rows; j++)  //and all columns of |L|
          L(i,j) = (L(i,j)*C(k,k) - C(i,k)*L(k,j))/denom; 

      for(i = k + 1; i < rows; i++) {      
        /* the following iteration uses and changes |C(i,k)| */
        RT temp = C(i,k); 
        for (j = k; j <= cols; j++)
          C(i,j) = (C(i,j)*C(k,k) - temp*C(k,j))/denom; 
      }
      denom = C(k,k); 
      #ifdef CGAL_LA_SELFTEST
      for(i = 0; i < rows; i++) { 
        for (j = 0; j < cols; j++) { 
          RT Sum = 0; 
          for (int l = 0; l < rows; l++) 
            Sum += L(i,l)*A(l, var[j]); 
          CGAL_assertion_msg( Sum  == C(i,j), 
            "linear_solver: L*A*P different from C"); 
        }
        RT Sum = 0; 
        for (int l = 0; l < rows; l++) 
          Sum += L(i,l)*b[l];
          CGAL_assertion_msg( Sum  == C(i,cols), 
            "linear_solver: L*A*P different from C"); 
      }
      #endif


    }
    else 
      break; 
  }


  /* at this point we have:
     |C| has an $rank \times rank$ upper triangular matrix 
     in its left upper corner; 
     |var| tells us the columns of |A| corresponding to the
     dependent variables; */
     
  columns = std::vector<int>(rank); 
  for(i = 0; i < rank; i++) 
    columns[i] = var[i]; 
  return rank;
}


template <class RT_, class AL_>  
int  Linear_algebraHd<RT_,AL_>::
rank(const Matrix& A)
{ 
  Vector b(A.row_dimension()); // zero - vector
  int i,j,k; // indices to step through the matrix
  int rows = A.row_dimension(); 
  int cols = A.column_dimension(); 

  /* at this point one might want to check whether the computation can
     be carried out with doubles, see section \ref{ optimization }. */

  CGAL_assertion_msg(b.dimension() == rows, 
    "linear_solver: b has wrong dimension");

  Matrix C(rows,cols + 1); 
  // the matrix in which we will calculate ($C = (A \vert b)$)
    
  /* copy |A| and |b| into |C| and L becomes the identity matrix */
  Matrix L(rows); // zero initialized
  for(i=0; i<rows; i++) { 
    for(j=0; j<cols; j++) 
      C(i,j)=A(i,j); 
    C(i,cols)=b[i]; 
    L(i,i) = 1; // diagonal elements are 1
  }


  std::vector<int> var(cols); 
  // column $j$ of |C| represents the |var[j]| - th variable
  // the array is indexed between |0| and |cols - 1|

  for(j=0; j<cols; j++)
    var[j]= j; // at the beginning, variable $x_j$ stands in column $j$

  RT_ denom = 1; // the determinant of an empty matrix is 1
  int sign = 1; // no interchanges yet
  int rank = 0; // we have not seen any non-zero row yet

  /* here comes the main loop */
  for(k=0; k<rows; k++) {   
    bool non_zero_found = false; 
    for(i = k; i < rows; i++) { // step through rows $k$ to |rows - 1|
      for (j = k ; j < cols && C(i,j) == 0; j++) ; 
      // step through columns |k| to |cols - 1|
      if (j < cols) {  
        non_zero_found = true; 
        break; 
      }
    }


    if ( non_zero_found ) { 
      rank++; //increase the rank
      if (i != k) { 
        sign = -sign; 
        /* we interchange rows |k| and |i| of |L| and |C| */
        L.swap_rows(i,k); C.swap_rows(i,k);
      }
      if (j != k) { 
        /* We interchange columns |k| and |j|, and
           store the exchange of variables in |var| */
        sign = - sign; 
        C.swap_columns(j,k);
        std::swap(var[k],var[j]); 
      }
       
      for(i = k + 1; i < rows; i++)  
        for (j = 0; j <  rows; j++)  //and all columns of |L|
          L(i,j) = (L(i,j)*C(k,k) - C(i,k)*L(k,j))/denom; 

      for(i = k + 1; i < rows; i++) {      
        /* the following iteration uses and changes |C(i,k)| */
        RT temp = C(i,k); 
        for (j = k; j <= cols; j++)
          C(i,j) = (C(i,j)*C(k,k) - temp*C(k,j))/denom; 
      }
      denom = C(k,k); 
      #ifdef CGAL_LA_SELFTEST
      for(i = 0; i < rows; i++) { 
        for (j = 0; j < cols; j++) { 
          RT Sum = 0; 
          for (int l = 0; l < rows; l++) 
            Sum += L(i,l)*A(l, var[j]); 
          CGAL_assertion_msg( Sum  == C(i,j), 
            "linear_solver: L*A*P different from C"); 
        }
        RT Sum = 0; 
        for (int l = 0; l < rows; l++) 
          Sum += L(i,l)*b[l];
          CGAL_assertion_msg( Sum  == C(i,cols), 
            "linear_solver: L*A*P different from C"); 
      }
      #endif


    }
    else 
      break; 
  }


  return rank; 
}

template <class RT_, class AL_>  
bool  Linear_algebraHd<RT_,AL_>::
inverse(const Matrix& A, Matrix& inverse, 
        RT_& D, Vector& c)
{ 
  if (A.row_dimension() != A.column_dimension())
    CGAL_assertion_msg(0,"inverse: only square matrices are legal inputs."); 
  Vector b(A.row_dimension()); // zero - vector
  int i,j,k; // indices to step through the matrix
  int rows = A.row_dimension(); 
  int cols = A.column_dimension(); 

  /* at this point one might want to check whether the computation can
     be carried out with doubles, see section \ref{ optimization }. */

  CGAL_assertion_msg(b.dimension() == rows, 
    "linear_solver: b has wrong dimension");

  Matrix C(rows,cols + 1); 
  // the matrix in which we will calculate ($C = (A \vert b)$)
    
  /* copy |A| and |b| into |C| and L becomes the identity matrix */
  Matrix L(rows); // zero initialized
  for(i=0; i<rows; i++) { 
    for(j=0; j<cols; j++) 
      C(i,j)=A(i,j); 
    C(i,cols)=b[i]; 
    L(i,i) = 1; // diagonal elements are 1
  }


  std::vector<int> var(cols); 
  // column $j$ of |C| represents the |var[j]| - th variable
  // the array is indexed between |0| and |cols - 1|

  for(j=0; j<cols; j++)
    var[j]= j; // at the beginning, variable $x_j$ stands in column $j$

  RT_ denom = 1; // the determinant of an empty matrix is 1
  int sign = 1; // no interchanges yet
  int rank = 0; // we have not seen any non-zero row yet

  /* here comes the main loop */
  for(k=0; k<rows; k++) {   
    bool non_zero_found = false; 
    for(i = k; i < rows; i++) { // step through rows $k$ to |rows - 1|
      for (j = k ; j < cols && C(i,j) == 0; j++) ; 
      // step through columns |k| to |cols - 1|
      if (j < cols) {  
        non_zero_found = true; 
        break; 
      }
    }


    if ( non_zero_found ) { 
      rank++; //increase the rank
      if (i != k) { 
        sign = -sign; 
        /* we interchange rows |k| and |i| of |L| and |C| */
        L.swap_rows(i,k); C.swap_rows(i,k);
      }
      if (j != k) { 
        /* We interchange columns |k| and |j|, and
           store the exchange of variables in |var| */
        sign = - sign; 
        C.swap_columns(j,k);
        std::swap(var[k],var[j]); 
      }
       
      for(i = k + 1; i < rows; i++)  
        for (j = 0; j <  rows; j++)  //and all columns of |L|
          L(i,j) = (L(i,j)*C(k,k) - C(i,k)*L(k,j))/denom; 

      for(i = k + 1; i < rows; i++) {      
        /* the following iteration uses and changes |C(i,k)| */
        RT temp = C(i,k); 
        for (j = k; j <= cols; j++)
          C(i,j) = (C(i,j)*C(k,k) - temp*C(k,j))/denom; 
      }
      denom = C(k,k); 
      #ifdef CGAL_LA_SELFTEST
      for(i = 0; i < rows; i++) { 
        for (j = 0; j < cols; j++) { 
          RT Sum = 0; 
          for (int l = 0; l < rows; l++) 
            Sum += L(i,l)*A(l, var[j]); 
          CGAL_assertion_msg( Sum  == C(i,j), 
            "linear_solver: L*A*P different from C"); 
        }
        RT Sum = 0; 
        for (int l = 0; l < rows; l++) 
          Sum += L(i,l)*b[l];
          CGAL_assertion_msg( Sum  == C(i,cols), 
            "linear_solver: L*A*P different from C"); 
      }
      #endif


    }
    else 
      break; 
  }


  
  if (rank < rows) { 
    // matrix is singular; return a vector $c$ with $c^T \cdot A = 0$.*/
    c = L.row(rows-1);
    return false; 
  }
  D = denom; 
  inverse = Matrix(rows); //square
  RT_ h; 
  for(i = 0; i <rows; i++) 
  {  // $i$-th column of inverse
    for (j = rows - 1; j >= 0; j--) { 
      h = L (j,i) * D; 
      for (int l = j + 1; l<rows; l++)  
        h -= C(j,l)*inverse(var[l],i); 
      inverse(var[j],i) = h / C(j,j); 
    }
  }  

  #ifdef CGAL_LA_SELFTEST
  if (A*inverse != Matrix(rows,Matrix::RT_val(1))*D)
    CGAL_assertion_msg(0,"inverse(): matrix inverse computed incorrectly."); 
  #endif      

  return true; 
}


template <class RT_, class AL_>  
int  Linear_algebraHd<RT_,AL_>::
homogeneous_linear_solver(const Matrix &A, 
                          Matrix& spanning_vectors)
/* returns the dimension of the solution space of the homogeneous system 
   $Ax = 0$. The columns of spanning\_vectors span the solution space. */
{ 
  Vector b(A.row_dimension()); // zero - vector
  RT_ D; 
  int i,j,k; // indices to step through the matrix
  int rows = A.row_dimension(); 
  int cols = A.column_dimension(); 

  /* at this point one might want to check whether the computation can
     be carried out with doubles, see section \ref{ optimization }. */

  CGAL_assertion_msg(b.dimension() == rows, 
    "linear_solver: b has wrong dimension");

  Matrix C(rows,cols + 1); 
  // the matrix in which we will calculate ($C = (A \vert b)$)
    
  /* copy |A| and |b| into |C| and L becomes the identity matrix */
  Matrix L(rows); // zero initialized
  for(i=0; i<rows; i++) { 
    for(j=0; j<cols; j++) 
      C(i,j)=A(i,j); 
    C(i,cols)=b[i]; 
    L(i,i) = 1; // diagonal elements are 1
  }


  std::vector<int> var(cols); 
  // column $j$ of |C| represents the |var[j]| - th variable
  // the array is indexed between |0| and |cols - 1|

  for(j=0; j<cols; j++)
    var[j]= j; // at the beginning, variable $x_j$ stands in column $j$

  RT_ denom = 1; // the determinant of an empty matrix is 1
  int sign = 1; // no interchanges yet
  int rank = 0; // we have not seen any non-zero row yet

  /* here comes the main loop */
  for(k=0; k<rows; k++) {   
    bool non_zero_found = false; 
    for(i = k; i < rows; i++) { // step through rows $k$ to |rows - 1|
      for (j = k ; j < cols && C(i,j) == 0; j++) ; 
      // step through columns |k| to |cols - 1|
      if (j < cols) {  
        non_zero_found = true; 
        break; 
      }
    }


    if ( non_zero_found ) { 
      rank++; //increase the rank
      if (i != k) { 
        sign = -sign; 
        /* we interchange rows |k| and |i| of |L| and |C| */
        L.swap_rows(i,k); C.swap_rows(i,k);
      }
      if (j != k) { 
        /* We interchange columns |k| and |j|, and
           store the exchange of variables in |var| */
        sign = - sign; 
        C.swap_columns(j,k);
        std::swap(var[k],var[j]); 
      }
       
      for(i = k + 1; i < rows; i++)  
        for (j = 0; j <  rows; j++)  //and all columns of |L|
          L(i,j) = (L(i,j)*C(k,k) - C(i,k)*L(k,j))/denom; 

      for(i = k + 1; i < rows; i++) {      
        /* the following iteration uses and changes |C(i,k)| */
        RT temp = C(i,k); 
        for (j = k; j <= cols; j++)
          C(i,j) = (C(i,j)*C(k,k) - temp*C(k,j))/denom; 
      }
      denom = C(k,k); 
      #ifdef CGAL_LA_SELFTEST
      for(i = 0; i < rows; i++) { 
        for (j = 0; j < cols; j++) { 
          RT Sum = 0; 
          for (int l = 0; l < rows; l++) 
            Sum += L(i,l)*A(l, var[j]); 
          CGAL_assertion_msg( Sum  == C(i,j), 
            "linear_solver: L*A*P different from C"); 
        }
        RT Sum = 0; 
        for (int l = 0; l < rows; l++) 
          Sum += L(i,l)*b[l];
          CGAL_assertion_msg( Sum  == C(i,cols), 
            "linear_solver: L*A*P different from C"); 
      }
      #endif


    }
    else 
      break; 
  }


  Vector x; 
  x = Vector(cols); 
  D = denom; 
  for(i = rank - 1; i >= 0; i--) { 
    RT_ h = C(i,cols) * D; 
    for (j = i + 1; j < rank; j++) { 
      h -= C(i,j)*x[var[j]]; 
    }
    x[var[i]]= h / C(i,i); 
  }
  #ifdef CGAL_LA_SELFTEST
    CGAL_assertion( (M*x).is_zero() );
  #endif

  int defect = cols - rank; // dimension of kernel
  spanning_vectors = Matrix(cols,defect); 
   
  if (defect > 0) { 
    for(int l=0; l < defect; l++) { 
      spanning_vectors(var[rank + l],l)=D; 
      for(i = rank - 1; i >= 0 ; i--) { 
        RT_ h = - C(i,rank + l)*D; 
        for ( j= i + 1; j<rank; j++)  
          h -= C(i,j)*spanning_vectors(var[j],l); 
        spanning_vectors(var[i],l)= h / C(i,i); 
      }
  #ifdef CGAL_LA_SELFTEST
        /* we check whether the $l$ - th spanning vector is a solution 
           of the homogeneous system */
        CGAL_assertion( (M*spanning_vectors.column(l)).is_zero() );
  #endif
    }
  }
; 
  return defect; 
}

template <class RT_, class AL_>  
bool  Linear_algebraHd<RT_,AL_>::
homogeneous_linear_solver(const Matrix& A, Vector& x)
/* returns true if the homogeneous system $Ax = 0$ has a non - trivial
   solution and false otherwise. */
{  
  x = Vector(A.row_dimension());
  Matrix spanning_vectors; 
  int dimension = homogeneous_linear_solver(A,spanning_vectors); 
  if (dimension == 0) return false; 

  /* return first column of |spanning_vectors| */
  for (int i = 0; i < spanning_vectors.row_dimension(); i++)  
    x[i] = spanning_vectors(i,0); 
  return true; 
}


template <class RT_, class AL_>  
typename Linear_algebraHd<RT_,AL_>::Matrix 
Linear_algebraHd<RT_,AL_>::transpose(const Matrix& M)
{ 
  int d1 = M.row_dimension(); 
  int d2 = M.column_dimension(); 
  Matrix result(d2,d1); 
  for(int i = 0; i < d2; i++)
    for(int j = 0; j < d1; j++)
      result(i,j) = M(j,i); 
  return result; 
}


CGAL_END_NAMESPACE
#endif //CGAL_LINEAR_ALGEBRAHD_C