From c6f7fd74627e525825ca32e567cbd677e60e5db4 Mon Sep 17 00:00:00 2001
From: wmayer <wmayer@users.sourceforge.net>
Date: Sun, 6 Oct 2013 13:11:11 +0200
Subject: [PATCH] 0000750: Endless loop in sketch solver

---
 src/Mod/Sketcher/App/freegcs/GCS.cpp | 113 +++++++++++++++++++++++++--
 1 file changed, 106 insertions(+), 7 deletions(-)

diff --git a/src/Mod/Sketcher/App/freegcs/GCS.cpp b/src/Mod/Sketcher/App/freegcs/GCS.cpp
index 5babc2786..ccea5d11d 100644
--- a/src/Mod/Sketcher/App/freegcs/GCS.cpp
+++ b/src/Mod/Sketcher/App/freegcs/GCS.cpp
@@ -30,6 +30,105 @@
 #include <boost/graph/adjacency_list.hpp>
 #include <boost/graph/connected_components.hpp>
 
+// http://sourceforge.net/apps/phpbb/free-cad/viewtopic.php?f=3&t=4651&start=40
+namespace Eigen {
+
+typedef Matrix<double,-1,-1,0,-1,-1> MatrixdType;
+template<>
+FullPivLU<MatrixdType>& FullPivLU<MatrixdType>::compute(const MatrixdType& matrix)
+{
+  m_isInitialized = true;
+  m_lu = matrix;
+
+  const Index size = matrix.diagonalSize();
+  const Index rows = matrix.rows();
+  const Index cols = matrix.cols();
+
+  // will store the transpositions, before we accumulate them at the end.
+  // can't accumulate on-the-fly because that will be done in reverse order for the rows.
+  m_rowsTranspositions.resize(matrix.rows());
+  m_colsTranspositions.resize(matrix.cols());
+  Index number_of_transpositions = 0; // number of NONTRIVIAL transpositions, i.e. m_rowsTranspositions[i]!=i
+
+  m_nonzero_pivots = size; // the generic case is that in which all pivots are nonzero (invertible case)
+  m_maxpivot = RealScalar(0);
+  RealScalar cutoff(0);
+
+  for(Index k = 0; k < size; ++k)
+  {
+    // First, we need to find the pivot.
+
+    // biggest coefficient in the remaining bottom-right corner (starting at row k, col k)
+    Index row_of_biggest_in_corner, col_of_biggest_in_corner;
+    RealScalar biggest_in_corner;
+    biggest_in_corner = m_lu.bottomRightCorner(rows-k, cols-k)
+                        .cwiseAbs()
+                        .maxCoeff(&row_of_biggest_in_corner, &col_of_biggest_in_corner);
+    row_of_biggest_in_corner += k; // correct the values! since they were computed in the corner,
+    col_of_biggest_in_corner += k; // need to add k to them.
+
+    // when k==0, biggest_in_corner is the biggest coeff absolute value in the original matrix
+    if(k == 0) cutoff = biggest_in_corner * NumTraits<Scalar>::epsilon();
+
+    // if the pivot (hence the corner) is "zero", terminate to avoid generating nan/inf values.
+    // Notice that using an exact comparison (biggest_in_corner==0) here, as Golub-van Loan do in
+    // their pseudo-code, results in numerical instability! The cutoff here has been validated
+    // by running the unit test 'lu' with many repetitions.
+    if(biggest_in_corner < cutoff)
+    {
+      // before exiting, make sure to initialize the still uninitialized transpositions
+      // in a sane state without destroying what we already have.
+      m_nonzero_pivots = k;
+      for(Index i = k; i < size; ++i)
+      {
+        m_rowsTranspositions.coeffRef(i) = i;
+        m_colsTranspositions.coeffRef(i) = i;
+      }
+      break;
+    }
+
+    if(biggest_in_corner > m_maxpivot) m_maxpivot = biggest_in_corner;
+
+    // Now that we've found the pivot, we need to apply the row/col swaps to
+    // bring it to the location (k,k).
+
+    m_rowsTranspositions.coeffRef(k) = row_of_biggest_in_corner;
+    m_colsTranspositions.coeffRef(k) = col_of_biggest_in_corner;
+    if(k != row_of_biggest_in_corner) {
+      m_lu.row(k).swap(m_lu.row(row_of_biggest_in_corner));
+      ++number_of_transpositions;
+    }
+    if(k != col_of_biggest_in_corner) {
+      m_lu.col(k).swap(m_lu.col(col_of_biggest_in_corner));
+      ++number_of_transpositions;
+    }
+
+    // Now that the pivot is at the right location, we update the remaining
+    // bottom-right corner by Gaussian elimination.
+
+    if(k<rows-1)
+      m_lu.col(k).tail(rows-k-1) /= m_lu.coeff(k,k);
+    if(k<size-1)
+      m_lu.block(k+1,k+1,rows-k-1,cols-k-1).noalias() -= m_lu.col(k).tail(rows-k-1) * m_lu.row(k).tail(cols-k-1);
+  }
+
+  // the main loop is over, we still have to accumulate the transpositions to find the
+  // permutations P and Q
+
+  m_p.setIdentity(rows);
+  for(Index k = size-1; k >= 0; --k)
+    m_p.applyTranspositionOnTheRight(k, m_rowsTranspositions.coeff(k));
+
+  m_q.setIdentity(cols);
+  for(Index k = 0; k < size; ++k)
+    m_q.applyTranspositionOnTheRight(k, m_colsTranspositions.coeff(k));
+
+  m_det_pq = (number_of_transpositions%2) ? -1 : 1;
+  return *this;
+}
+
+} // Eigen
+
 namespace GCS
 {
 
@@ -42,7 +141,7 @@ typedef boost::adjacency_list <boost::vecS, boost::vecS, boost::undirectedS> Gra
 // System
 System::System()
 : plist(0), clist(0),
-  c2p(), p2c(),
+  c2p(), p2c(),
   subSystems(0), subSystemsAux(0),
   reference(0),
   hasUnknowns(false), hasDiagnosis(false), isInit(false)
@@ -53,7 +152,7 @@ System::System(std::vector<Constraint *> clist_)
 : plist(0),
   c2p(), p2c(),
   subSystems(0), subSystemsAux(0),
-  reference(0),
+  reference(0),
   hasUnknowns(false), hasDiagnosis(false), isInit(false)
 {
     // create own (shallow) copy of constraints
@@ -822,14 +921,14 @@ int System::solve_BFGS(SubSystem *subsys, bool isFine)
     double convergence = isFine ? XconvergenceFine : XconvergenceRough;
     int maxIterNumber = MaxIterations * xsize;
     double divergingLim = 1e6*err + 1e12;
-
+
     for (int iter=1; iter < maxIterNumber; iter++) {
 
         if (h.norm() <= convergence || err <= smallF)
             break;
         if (err > divergingLim || err != err) // check for diverging and NaN
             break;
-
+
         y = grad;
         subsys->calcGrad(grad);
         y = grad - y; // = grad - gradold
@@ -1093,7 +1192,7 @@ int System::solve_DL(SubSystem* subsys)
         if (stop)
             break;
 
-// it didn't work in some tests
+// it didn't work in some tests
 //        // restrict h_dl according to maxStep
 //        double scale = subsys->maxStep(h_dl);
 //        if (scale < 1.)
@@ -1369,7 +1468,7 @@ int System::diagnose()
         int paramsNum = qrJT.rows();
         int constrNum = qrJT.cols();
         int rank = qrJT.rank();
-
+
         Eigen::MatrixXd R;
         if (constrNum >= paramsNum)
             R = qrJT.matrixQR().triangularView<Eigen::Upper>();
@@ -1508,7 +1607,7 @@ int System::diagnose()
                 return dofs;
             }
         }
-
+
         hasDiagnosis = true;
         dofs = paramsNum - rank;
         return dofs;