SimplicialCholesky_impl.h
Go to the documentation of this file.
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2008-2012 Gael Guennebaud <gael.guennebaud@inria.fr>
00005 
00006 /*
00007 
00008 NOTE: thes functions vave been adapted from the LDL library:
00009 
00010 LDL Copyright (c) 2005 by Timothy A. Davis.  All Rights Reserved.
00011 
00012 LDL License:
00013 
00014     Your use or distribution of LDL or any modified version of
00015     LDL implies that you agree to this License.
00016 
00017     This library is free software; you can redistribute it and/or
00018     modify it under the terms of the GNU Lesser General Public
00019     License as published by the Free Software Foundation; either
00020     version 2.1 of the License, or (at your option) any later version.
00021 
00022     This library is distributed in the hope that it will be useful,
00023     but WITHOUT ANY WARRANTY; without even the implied warranty of
00024     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00025     Lesser General Public License for more details.
00026 
00027     You should have received a copy of the GNU Lesser General Public
00028     License along with this library; if not, write to the Free Software
00029     Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301
00030     USA
00031 
00032     Permission is hereby granted to use or copy this program under the
00033     terms of the GNU LGPL, provided that the Copyright, this License,
00034     and the Availability of the original version is retained on all copies.
00035     User documentation of any code that uses this code or any modified
00036     version of this code must cite the Copyright, this License, the
00037     Availability note, and "Used by permission." Permission to modify
00038     the code and to distribute modified code is granted, provided the
00039     Copyright, this License, and the Availability note are retained,
00040     and a notice that the code was modified is included.
00041  */
00042 
00043 #include "../Core/util/NonMPL2.h"
00044 
00045 #ifndef EIGEN_SIMPLICIAL_CHOLESKY_IMPL_H
00046 #define EIGEN_SIMPLICIAL_CHOLESKY_IMPL_H
00047 
00048 namespace Eigen {
00049 
00050 template<typename Derived>
00051 void SimplicialCholeskyBase<Derived>::analyzePattern_preordered(const CholMatrixType& ap, bool doLDLT)
00052 {
00053   const Index size = ap.rows();
00054   m_matrix.resize(size, size);
00055   m_parent.resize(size);
00056   m_nonZerosPerCol.resize(size);
00057 
00058   ei_declare_aligned_stack_constructed_variable(Index, tags, size, 0);
00059 
00060   for(Index k = 0; k < size; ++k)
00061   {
00062     /* L(k,:) pattern: all nodes reachable in etree from nz in A(0:k-1,k) */
00063     m_parent[k] = -1;             /* parent of k is not yet known */
00064     tags[k] = k;                  /* mark node k as visited */
00065     m_nonZerosPerCol[k] = 0;      /* count of nonzeros in column k of L */
00066     for(typename CholMatrixType::InnerIterator it(ap,k); it; ++it)
00067     {
00068       Index i = it.index();
00069       if(i < k)
00070       {
00071         /* follow path from i to root of etree, stop at flagged node */
00072         for(; tags[i] != k; i = m_parent[i])
00073         {
00074           /* find parent of i if not yet determined */
00075           if (m_parent[i] == -1)
00076             m_parent[i] = k;
00077           m_nonZerosPerCol[i]++;        /* L (k,i) is nonzero */
00078           tags[i] = k;                  /* mark i as visited */
00079         }
00080       }
00081     }
00082   }
00083 
00084   /* construct Lp index array from m_nonZerosPerCol column counts */
00085   Index* Lp = m_matrix.outerIndexPtr();
00086   Lp[0] = 0;
00087   for(Index k = 0; k < size; ++k)
00088     Lp[k+1] = Lp[k] + m_nonZerosPerCol[k] + (doLDLT ? 0 : 1);
00089 
00090   m_matrix.resizeNonZeros(Lp[size]);
00091 
00092   m_isInitialized     = true;
00093   m_info              = Success;
00094   m_analysisIsOk      = true;
00095   m_factorizationIsOk = false;
00096 }
00097 
00098 
00099 template<typename Derived>
00100 template<bool DoLDLT>
00101 void SimplicialCholeskyBase<Derived>::factorize_preordered(const CholMatrixType& ap)
00102 {
00103   using std::sqrt;
00104 
00105   eigen_assert(m_analysisIsOk && "You must first call analyzePattern()");
00106   eigen_assert(ap.rows()==ap.cols());
00107   const Index size = ap.rows();
00108   eigen_assert(m_parent.size()==size);
00109   eigen_assert(m_nonZerosPerCol.size()==size);
00110 
00111   const Index* Lp = m_matrix.outerIndexPtr();
00112   Index* Li = m_matrix.innerIndexPtr();
00113   Scalar* Lx = m_matrix.valuePtr();
00114 
00115   ei_declare_aligned_stack_constructed_variable(Scalar, y, size, 0);
00116   ei_declare_aligned_stack_constructed_variable(Index,  pattern, size, 0);
00117   ei_declare_aligned_stack_constructed_variable(Index,  tags, size, 0);
00118 
00119   bool ok = true;
00120   m_diag.resize(DoLDLT ? size : 0);
00121 
00122   for(Index k = 0; k < size; ++k)
00123   {
00124     // compute nonzero pattern of kth row of L, in topological order
00125     y[k] = 0.0;                     // Y(0:k) is now all zero
00126     Index top = size;               // stack for pattern is empty
00127     tags[k] = k;                    // mark node k as visited
00128     m_nonZerosPerCol[k] = 0;        // count of nonzeros in column k of L
00129     for(typename MatrixType::InnerIterator it(ap,k); it; ++it)
00130     {
00131       Index i = it.index();
00132       if(i <= k)
00133       {
00134         y[i] += numext::conj(it.value());            /* scatter A(i,k) into Y (sum duplicates) */
00135         Index len;
00136         for(len = 0; tags[i] != k; i = m_parent[i])
00137         {
00138           pattern[len++] = i;     /* L(k,i) is nonzero */
00139           tags[i] = k;            /* mark i as visited */
00140         }
00141         while(len > 0)
00142           pattern[--top] = pattern[--len];
00143       }
00144     }
00145 
00146     /* compute numerical values kth row of L (a sparse triangular solve) */
00147 
00148     RealScalar d = numext::real(y[k]) * m_shiftScale + m_shiftOffset;    // get D(k,k), apply the shift function, and clear Y(k)
00149     y[k] = 0.0;
00150     for(; top < size; ++top)
00151     {
00152       Index i = pattern[top];       /* pattern[top:n-1] is pattern of L(:,k) */
00153       Scalar yi = y[i];             /* get and clear Y(i) */
00154       y[i] = 0.0;
00155 
00156       /* the nonzero entry L(k,i) */
00157       Scalar l_ki;
00158       if(DoLDLT)
00159         l_ki = yi / m_diag[i];
00160       else
00161         yi = l_ki = yi / Lx[Lp[i]];
00162 
00163       Index p2 = Lp[i] + m_nonZerosPerCol[i];
00164       Index p;
00165       for(p = Lp[i] + (DoLDLT ? 0 : 1); p < p2; ++p)
00166         y[Li[p]] -= numext::conj(Lx[p]) * yi;
00167       d -= numext::real(l_ki * numext::conj(yi));
00168       Li[p] = k;                          /* store L(k,i) in column form of L */
00169       Lx[p] = l_ki;
00170       ++m_nonZerosPerCol[i];              /* increment count of nonzeros in col i */
00171     }
00172     if(DoLDLT)
00173     {
00174       m_diag[k] = d;
00175       if(d == RealScalar(0))
00176       {
00177         ok = false;                         /* failure, D(k,k) is zero */
00178         break;
00179       }
00180     }
00181     else
00182     {
00183       Index p = Lp[k] + m_nonZerosPerCol[k]++;
00184       Li[p] = k ;                /* store L(k,k) = sqrt (d) in column k */
00185       if(d <= RealScalar(0)) {
00186         ok = false;              /* failure, matrix is not positive definite */
00187         break;
00188       }
00189       Lx[p] = sqrt(d) ;
00190     }
00191   }
00192 
00193   m_info = ok ? Success : NumericalIssue;
00194   m_factorizationIsOk = true;
00195 }
00196 
00197 } // end namespace Eigen
00198 
00199 #endif // EIGEN_SIMPLICIAL_CHOLESKY_IMPL_H


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Thu Aug 27 2015 12:00:14