1 /* 2 * This file is part of CasADi. 3 * 4 * CasADi -- A symbolic framework for dynamic optimization. 5 * Copyright (C) 2010-2014 Joel Andersson, Joris Gillis, Moritz Diehl, 6 * K.U. Leuven. All rights reserved. 7 * Copyright (C) 2011-2014 Greg Horn 8 * Copyright (C) 2005-2013 Timothy A. Davis 9 * 10 * CasADi is free software; you can redistribute it and/or 11 * modify it under the terms of the GNU Lesser General Public 12 * License as published by the Free Software Foundation; either 13 * version 3 of the License, or (at your option) any later version. 14 * 15 * CasADi is distributed in the hope that it will be useful, 16 * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 18 * Lesser General Public License for more details. 19 * 20 * You should have received a copy of the GNU Lesser General Public 21 * License along with CasADi; if not, write to the Free Software 22 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 23 * 24 */ 25 26 27 #include "sparsity_internal.hpp" 28 #include "im.hpp" 29 #include "casadi_misc.hpp" 30 #include "sparse_storage_impl.hpp" 31 #include "serializing_stream.hpp" 32 #include <climits> 33 34 #define CASADI_THROW_ERROR(FNAME, WHAT) \ 35 throw CasadiException("Error in Sparsity::" FNAME " at " + CASADI_WHERE + ":\n"\ 36 + std::string(WHAT)); 37 38 using namespace std; 39 40 namespace casadi { 41 // Instantiate templates 42 template class SparseStorage<Sparsity>; 43 44 /// \cond INTERNAL 45 // Singletons 46 class EmptySparsity : public Sparsity { 47 public: EmptySparsity()48 EmptySparsity() { 49 const casadi_int colind[1] = {0}; 50 own(new SparsityInternal(0, 0, colind, nullptr)); 51 } 52 }; 53 54 class ScalarSparsity : public Sparsity { 55 public: ScalarSparsity()56 ScalarSparsity() { 57 const casadi_int colind[2] = {0, 1}; 58 const casadi_int row[1] = {0}; 59 own(new SparsityInternal(1, 1, colind, row)); 60 } 61 }; 62 63 class ScalarSparseSparsity : public Sparsity { 64 public: ScalarSparseSparsity()65 ScalarSparseSparsity() { 66 const casadi_int colind[2] = {0, 0}; 67 const casadi_int row[1] = {0}; 68 own(new SparsityInternal(1, 1, colind, row)); 69 } 70 }; 71 /// \endcond 72 Sparsity(casadi_int dummy)73 Sparsity::Sparsity(casadi_int dummy) { 74 casadi_assert_dev(dummy==0); 75 } 76 create(SparsityInternal * node)77 Sparsity Sparsity::create(SparsityInternal *node) { 78 Sparsity ret; 79 ret.own(node); 80 return ret; 81 } 82 Sparsity(casadi_int nrow,casadi_int ncol)83 Sparsity::Sparsity(casadi_int nrow, casadi_int ncol) { 84 casadi_assert_dev(nrow>=0); 85 casadi_assert_dev(ncol>=0); 86 std::vector<casadi_int> row, colind(ncol+1, 0); 87 assign_cached(nrow, ncol, colind, row); 88 } 89 Sparsity(const std::pair<casadi_int,casadi_int> & rc)90 Sparsity::Sparsity(const std::pair<casadi_int, casadi_int>& rc) { 91 casadi_assert_dev(rc.first>=0); 92 casadi_assert_dev(rc.second>=0); 93 std::vector<casadi_int> row, colind(rc.second+1, 0); 94 assign_cached(rc.first, rc.second, colind, row); 95 } 96 Sparsity(casadi_int nrow,casadi_int ncol,const std::vector<casadi_int> & colind,const std::vector<casadi_int> & row,bool order_rows)97 Sparsity::Sparsity(casadi_int nrow, casadi_int ncol, const std::vector<casadi_int>& colind, 98 const std::vector<casadi_int>& row, bool order_rows) { 99 casadi_assert_dev(nrow>=0); 100 casadi_assert_dev(ncol>=0); 101 assign_cached(nrow, ncol, colind, row, order_rows); 102 } 103 Sparsity(casadi_int nrow,casadi_int ncol,const casadi_int * colind,const casadi_int * row,bool order_rows)104 Sparsity::Sparsity(casadi_int nrow, casadi_int ncol, 105 const casadi_int* colind, const casadi_int* row, bool order_rows) { 106 casadi_assert_dev(nrow>=0); 107 casadi_assert_dev(ncol>=0); 108 if (colind==nullptr || colind[ncol]==nrow*ncol) { 109 *this = dense(nrow, ncol); 110 } else { 111 vector<casadi_int> colindv(colind, colind+ncol+1); 112 vector<casadi_int> rowv(row, row+colind[ncol]); 113 assign_cached(nrow, ncol, colindv, rowv, order_rows); 114 } 115 } 116 operator ->() const117 const SparsityInternal* Sparsity::operator->() const { 118 return static_cast<const SparsityInternal*>(SharedObject::operator->()); 119 } 120 operator *() const121 const SparsityInternal& Sparsity::operator*() const { 122 return *static_cast<const SparsityInternal*>(get()); 123 } 124 test_cast(const SharedObjectInternal * ptr)125 bool Sparsity::test_cast(const SharedObjectInternal* ptr) { 126 return dynamic_cast<const SparsityInternal*>(ptr)!=nullptr; 127 } 128 size1() const129 casadi_int Sparsity::size1() const { 130 return (*this)->size1(); 131 } 132 size2() const133 casadi_int Sparsity::size2() const { 134 return (*this)->size2(); 135 } 136 numel() const137 casadi_int Sparsity::numel() const { 138 return (*this)->numel(); 139 } 140 density() const141 double Sparsity::density() const { 142 double r = 100; 143 r *= static_cast<double>(nnz()); 144 r /= static_cast<double>(size1()); 145 r /= static_cast<double>(size2()); 146 return r; 147 } 148 is_empty(bool both) const149 bool Sparsity::is_empty(bool both) const { 150 return (*this)->is_empty(both); 151 } 152 nnz() const153 casadi_int Sparsity::nnz() const { 154 return (*this)->nnz(); 155 } 156 size() const157 std::pair<casadi_int, casadi_int> Sparsity::size() const { 158 return (*this)->size(); 159 } 160 size(casadi_int axis) const161 casadi_int Sparsity::size(casadi_int axis) const { 162 switch (axis) { 163 case 1: return size1(); 164 case 2: return size2(); 165 } 166 casadi_error("Axis must be 1 or 2."); 167 } 168 row() const169 const casadi_int* Sparsity::row() const { 170 return (*this)->row(); 171 } 172 colind() const173 const casadi_int* Sparsity::colind() const { 174 return (*this)->colind(); 175 } 176 row(casadi_int el) const177 casadi_int Sparsity::row(casadi_int el) const { 178 if (el<0 || el>=nnz()) { 179 throw std::out_of_range("Sparsity::row: Index " + str(el) 180 + " out of range [0," + str(nnz()) + ")"); 181 } 182 return row()[el]; 183 } 184 colind(casadi_int cc) const185 casadi_int Sparsity::colind(casadi_int cc) const { 186 if (cc<0 || cc>size2()) { 187 throw std::out_of_range("Sparsity::colind: Index " 188 + str(cc) + " out of range [0," + str(size2()) + "]"); 189 } 190 return colind()[cc]; 191 } 192 resize(casadi_int nrow,casadi_int ncol)193 void Sparsity::resize(casadi_int nrow, casadi_int ncol) { 194 if (size1()!=nrow || size2() != ncol) { 195 *this = (*this)->_resize(nrow, ncol); 196 } 197 } 198 add_nz(casadi_int rr,casadi_int cc)199 casadi_int Sparsity::add_nz(casadi_int rr, casadi_int cc) { 200 // If negative index, count from the back 201 if (rr<0) rr += size1(); 202 if (cc<0) cc += size2(); 203 204 // Check consistency 205 casadi_assert(rr>=0 && rr<size1(), "Row index out of bounds"); 206 casadi_assert(cc>=0 && cc<size2(), "Column index out of bounds"); 207 208 // Quick return if matrix is dense 209 if (is_dense()) return rr+cc*size1(); 210 211 // Get sparsity pattern 212 casadi_int size1=this->size1(), size2=this->size2(), nnz=this->nnz(); 213 const casadi_int *colind = this->colind(), *row = this->row(); 214 215 // Quick return if we are adding an element to the end 216 if (colind[cc]==nnz || (colind[cc+1]==nnz && row[nnz-1]<rr)) { 217 std::vector<casadi_int> rowv(nnz+1); 218 copy(row, row+nnz, rowv.begin()); 219 rowv[nnz] = rr; 220 std::vector<casadi_int> colindv(colind, colind+size2+1); 221 for (casadi_int c=cc; c<size2; ++c) colindv[c+1]++; 222 assign_cached(size1, size2, colindv, rowv); 223 return rowv.size()-1; 224 } 225 226 // go to the place where the element should be 227 casadi_int ind; 228 for (ind=colind[cc]; ind<colind[cc+1]; ++ind) { // better: loop from the back to the front 229 if (row[ind] == rr) { 230 return ind; // element exists 231 } else if (row[ind] > rr) { 232 break; // break at the place where the element should be added 233 } 234 } 235 236 // insert the element 237 std::vector<casadi_int> rowv = get_row(), colindv = get_colind(); 238 rowv.insert(rowv.begin()+ind, rr); 239 for (casadi_int c=cc+1; c<size2+1; ++c) colindv[c]++; 240 241 // Return the location of the new element 242 assign_cached(size1, size2, colindv, rowv); 243 return ind; 244 } 245 has_nz(casadi_int rr,casadi_int cc) const246 bool Sparsity::has_nz(casadi_int rr, casadi_int cc) const { 247 return get_nz(rr, cc)!=-1; 248 } 249 250 get_nz(casadi_int rr,casadi_int cc) const251 casadi_int Sparsity::get_nz(casadi_int rr, casadi_int cc) const { 252 return (*this)->get_nz(rr, cc); 253 } 254 reshape(const Sparsity & x,const Sparsity & sp)255 Sparsity Sparsity::reshape(const Sparsity& x, const Sparsity& sp) { 256 casadi_assert_dev(x.is_reshape(sp)); 257 return sp; 258 } 259 reshape(const Sparsity & x,casadi_int nrow,casadi_int ncol)260 Sparsity Sparsity::reshape(const Sparsity& x, casadi_int nrow, casadi_int ncol) { 261 return x->_reshape(nrow, ncol); 262 } 263 get_nz(const std::vector<casadi_int> & rr,const std::vector<casadi_int> & cc) const264 std::vector<casadi_int> Sparsity::get_nz(const std::vector<casadi_int>& rr, 265 const std::vector<casadi_int>& cc) const { 266 return (*this)->get_nz(rr, cc); 267 } 268 is_scalar(bool scalar_and_dense) const269 bool Sparsity::is_scalar(bool scalar_and_dense) const { 270 return (*this)->is_scalar(scalar_and_dense); 271 } 272 is_dense() const273 bool Sparsity::is_dense() const { 274 return (*this)->is_dense(); 275 } 276 is_diag() const277 bool Sparsity::is_diag() const { 278 return (*this)->is_diag(); 279 } 280 is_row() const281 bool Sparsity::is_row() const { 282 return (*this)->is_row(); 283 } 284 is_column() const285 bool Sparsity::is_column() const { 286 return (*this)->is_column(); 287 } 288 is_vector() const289 bool Sparsity::is_vector() const { 290 return (*this)->is_vector(); 291 } 292 is_square() const293 bool Sparsity::is_square() const { 294 return (*this)->is_square(); 295 } 296 is_symmetric() const297 bool Sparsity::is_symmetric() const { 298 return (*this)->is_symmetric(); 299 } 300 is_tril() const301 bool Sparsity::is_tril() const { 302 return (*this)->is_tril(); 303 } 304 is_triu() const305 bool Sparsity::is_triu() const { 306 return (*this)->is_triu(); 307 } 308 sub(const std::vector<casadi_int> & rr,const Sparsity & sp,std::vector<casadi_int> & mapping,bool ind1) const309 Sparsity Sparsity::sub(const std::vector<casadi_int>& rr, const Sparsity& sp, 310 std::vector<casadi_int>& mapping, bool ind1) const { 311 return (*this)->sub(rr, *sp, mapping, ind1); 312 } 313 sub(const std::vector<casadi_int> & rr,const std::vector<casadi_int> & cc,std::vector<casadi_int> & mapping,bool ind1) const314 Sparsity Sparsity::sub(const std::vector<casadi_int>& rr, const std::vector<casadi_int>& cc, 315 std::vector<casadi_int>& mapping, bool ind1) const { 316 return (*this)->sub(rr, cc, mapping, ind1); 317 } 318 erase(const std::vector<casadi_int> & rr,const std::vector<casadi_int> & cc,bool ind1)319 std::vector<casadi_int> Sparsity::erase(const std::vector<casadi_int>& rr, 320 const std::vector<casadi_int>& cc, bool ind1) { 321 vector<casadi_int> mapping; 322 *this = (*this)->_erase(rr, cc, ind1, mapping); 323 return mapping; 324 } 325 erase(const std::vector<casadi_int> & rr,bool ind1)326 std::vector<casadi_int> Sparsity::erase(const std::vector<casadi_int>& rr, bool ind1) { 327 vector<casadi_int> mapping; 328 *this = (*this)->_erase(rr, ind1, mapping); 329 return mapping; 330 } 331 nnz_lower(bool strictly) const332 casadi_int Sparsity::nnz_lower(bool strictly) const { 333 return (*this)->nnz_lower(strictly); 334 } 335 nnz_upper(bool strictly) const336 casadi_int Sparsity::nnz_upper(bool strictly) const { 337 return (*this)->nnz_upper(strictly); 338 } 339 nnz_diag() const340 casadi_int Sparsity::nnz_diag() const { 341 return (*this)->nnz_diag(); 342 } 343 get_colind() const344 std::vector<casadi_int> Sparsity::get_colind() const { 345 return (*this)->get_colind(); 346 } 347 get_col() const348 std::vector<casadi_int> Sparsity::get_col() const { 349 return (*this)->get_col(); 350 } 351 get_row() const352 std::vector<casadi_int> Sparsity::get_row() const { 353 return (*this)->get_row(); 354 } 355 get_ccs(std::vector<casadi_int> & colind,std::vector<casadi_int> & row) const356 void Sparsity::get_ccs(std::vector<casadi_int>& colind, std::vector<casadi_int>& row) const { 357 colind = get_colind(); 358 row = get_row(); 359 } 360 get_crs(std::vector<casadi_int> & rowind,std::vector<casadi_int> & col) const361 void Sparsity::get_crs(std::vector<casadi_int>& rowind, std::vector<casadi_int>& col) const { 362 T().get_ccs(rowind, col); 363 } 364 get_triplet(std::vector<casadi_int> & row,std::vector<casadi_int> & col) const365 void Sparsity::get_triplet(std::vector<casadi_int>& row, std::vector<casadi_int>& col) const { 366 row = get_row(); 367 col = get_col(); 368 } 369 transpose(std::vector<casadi_int> & mapping,bool invert_mapping) const370 Sparsity Sparsity::transpose(std::vector<casadi_int>& mapping, bool invert_mapping) const { 371 return (*this)->transpose(mapping, invert_mapping); 372 } 373 T() const374 Sparsity Sparsity::T() const { 375 return (*this)->T(); 376 } 377 combine(const Sparsity & y,bool f0x_is_zero,bool fx0_is_zero,std::vector<unsigned char> & mapping) const378 Sparsity Sparsity::combine(const Sparsity& y, bool f0x_is_zero, 379 bool fx0_is_zero, 380 std::vector<unsigned char>& mapping) const { 381 return (*this)->combine(y, f0x_is_zero, fx0_is_zero, mapping); 382 } 383 combine(const Sparsity & y,bool f0x_is_zero,bool fx0_is_zero) const384 Sparsity Sparsity::combine(const Sparsity& y, bool f0x_is_zero, 385 bool fx0_is_zero) const { 386 return (*this)->combine(y, f0x_is_zero, fx0_is_zero); 387 } 388 unite(const Sparsity & y,std::vector<unsigned char> & mapping) const389 Sparsity Sparsity::unite(const Sparsity& y, std::vector<unsigned char>& mapping) const { 390 return (*this)->combine(y, false, false, mapping); 391 } 392 unite(const Sparsity & y) const393 Sparsity Sparsity::unite(const Sparsity& y) const { 394 return (*this)->combine(y, false, false); 395 } 396 intersect(const Sparsity & y,std::vector<unsigned char> & mapping) const397 Sparsity Sparsity::intersect(const Sparsity& y, 398 std::vector<unsigned char>& mapping) const { 399 return (*this)->combine(y, true, true, mapping); 400 } 401 intersect(const Sparsity & y) const402 Sparsity Sparsity::intersect(const Sparsity& y) const { 403 return (*this)->combine(y, true, true); 404 } 405 is_subset(const Sparsity & rhs) const406 bool Sparsity::is_subset(const Sparsity& rhs) const { 407 return (*this)->is_subset(rhs); 408 } 409 mtimes(const Sparsity & x,const Sparsity & y)410 Sparsity Sparsity::mtimes(const Sparsity& x, const Sparsity& y) { 411 // Check matching dimensions 412 casadi_assert(x.size2()==y.size1(), 413 "Matrix product with incompatible dimensions. Lhs is " 414 + x.dim() + " and rhs is " + y.dim() + "."); 415 416 return x->_mtimes(y); 417 } 418 is_stacked(const Sparsity & y,casadi_int n) const419 bool Sparsity::is_stacked(const Sparsity& y, casadi_int n) const { 420 return (*this)->is_stacked(y, n); 421 } 422 is_equal(const Sparsity & y) const423 bool Sparsity::is_equal(const Sparsity& y) const { 424 return (*this)->is_equal(y); 425 } 426 is_equal(casadi_int nrow,casadi_int ncol,const std::vector<casadi_int> & colind,const std::vector<casadi_int> & row) const427 bool Sparsity::is_equal(casadi_int nrow, casadi_int ncol, const std::vector<casadi_int>& colind, 428 const std::vector<casadi_int>& row) const { 429 return (*this)->is_equal(nrow, ncol, colind, row); 430 } 431 is_equal(casadi_int nrow,casadi_int ncol,const casadi_int * colind,const casadi_int * row) const432 bool Sparsity::is_equal(casadi_int nrow, casadi_int ncol, 433 const casadi_int* colind, const casadi_int* row) const { 434 return (*this)->is_equal(nrow, ncol, colind, row); 435 } 436 operator +(const Sparsity & b) const437 Sparsity Sparsity::operator+(const Sparsity& b) const { 438 return unite(b); 439 } 440 operator *(const Sparsity & b) const441 Sparsity Sparsity::operator*(const Sparsity& b) const { 442 std::vector< unsigned char > mapping; 443 return intersect(b, mapping); 444 } 445 pattern_inverse() const446 Sparsity Sparsity::pattern_inverse() const { 447 return (*this)->pattern_inverse(); 448 } 449 append(const Sparsity & sp)450 void Sparsity::append(const Sparsity& sp) { 451 if (sp.size1()==0 && sp.size2()==0) { 452 // Appending pattern is empty 453 return; 454 } else if (size1()==0 && size2()==0) { 455 // This is empty 456 *this = sp; 457 } else { 458 casadi_assert(size2()==sp.size2(), 459 "Sparsity::append: Dimension mismatch. " 460 "You attempt to append a shape " + sp.dim() 461 + " to a shape " + dim() 462 + ". The number of columns must match."); 463 if (sp.size1()==0) { 464 // No rows to add 465 return; 466 } else if (size1()==0) { 467 // No rows before 468 *this = sp; 469 } else if (is_column()) { 470 // Append to vector (inefficient) 471 *this = (*this)->_appendVector(*sp); 472 } else { 473 // Append to matrix (inefficient) 474 *this = vertcat({*this, sp}); 475 } 476 } 477 } 478 appendColumns(const Sparsity & sp)479 void Sparsity::appendColumns(const Sparsity& sp) { 480 if (sp.size1()==0 && sp.size2()==0) { 481 // Appending pattern is empty 482 return; 483 } else if (size1()==0 && size2()==0) { 484 // This is empty 485 *this = sp; 486 } else { 487 casadi_assert(size1()==sp.size1(), 488 "Sparsity::appendColumns: Dimension mismatch. You attempt to " 489 "append a shape " + sp.dim() + " to a shape " 490 + dim() + ". The number of rows must match."); 491 if (sp.size2()==0) { 492 // No columns to add 493 return; 494 } else if (size2()==0) { 495 // No columns before 496 *this = sp; 497 } else { 498 // Append to matrix (expensive) 499 *this = (*this)->_appendColumns(*sp); 500 } 501 } 502 } 503 getCache()504 Sparsity::CachingMap& Sparsity::getCache() { 505 static CachingMap ret; 506 return ret; 507 } 508 getScalar()509 const Sparsity& Sparsity::getScalar() { 510 static ScalarSparsity ret; 511 return ret; 512 } 513 getScalarSparse()514 const Sparsity& Sparsity::getScalarSparse() { 515 static ScalarSparseSparsity ret; 516 return ret; 517 } 518 getEmpty()519 const Sparsity& Sparsity::getEmpty() { 520 static EmptySparsity ret; 521 return ret; 522 } 523 enlarge(casadi_int nrow,casadi_int ncol,const std::vector<casadi_int> & rr,const std::vector<casadi_int> & cc,bool ind1)524 void Sparsity::enlarge(casadi_int nrow, casadi_int ncol, const std::vector<casadi_int>& rr, 525 const std::vector<casadi_int>& cc, bool ind1) { 526 enlargeColumns(ncol, cc, ind1); 527 enlargeRows(nrow, rr, ind1); 528 } 529 enlargeColumns(casadi_int ncol,const std::vector<casadi_int> & cc,bool ind1)530 void Sparsity::enlargeColumns(casadi_int ncol, const std::vector<casadi_int>& cc, bool ind1) { 531 casadi_assert_dev(cc.size() == size2()); 532 if (cc.empty()) { 533 *this = Sparsity(size1(), ncol); 534 } else { 535 *this = (*this)->_enlargeColumns(ncol, cc, ind1); 536 } 537 } 538 enlargeRows(casadi_int nrow,const std::vector<casadi_int> & rr,bool ind1)539 void Sparsity::enlargeRows(casadi_int nrow, const std::vector<casadi_int>& rr, bool ind1) { 540 casadi_assert_dev(rr.size() == size1()); 541 if (rr.empty()) { 542 *this = Sparsity(nrow, size2()); 543 } else { 544 *this = (*this)->_enlargeRows(nrow, rr, ind1); 545 } 546 } 547 diag(casadi_int nrow,casadi_int ncol)548 Sparsity Sparsity::diag(casadi_int nrow, casadi_int ncol) { 549 // Smallest dimension 550 casadi_int n = min(nrow, ncol); 551 552 // Column offset 553 vector<casadi_int> colind(ncol+1, n); 554 for (casadi_int cc=0; cc<n; ++cc) colind[cc] = cc; 555 556 // Row 557 vector<casadi_int> row = range(n); 558 559 // Create pattern from vectors 560 return Sparsity(nrow, ncol, colind, row); 561 } 562 makeDense(std::vector<casadi_int> & mapping) const563 Sparsity Sparsity::makeDense(std::vector<casadi_int>& mapping) const { 564 return (*this)->makeDense(mapping); 565 } 566 dim(bool with_nz) const567 std::string Sparsity::dim(bool with_nz) const { 568 return (*this)->dim(with_nz); 569 } 570 postfix_dim() const571 std::string Sparsity::postfix_dim() const { 572 if (is_dense()) { 573 if (is_scalar()) { 574 return ""; 575 } else if (is_empty(true)) { 576 return "[]"; 577 } else if (is_column()) { 578 return "[" + str(size1()) + "]"; 579 } else { 580 return "[" + dim(false) + "]"; 581 } 582 } else { 583 return "[" + dim(true) + "]"; 584 } 585 } 586 repr_el(casadi_int k) const587 std::string Sparsity::repr_el(casadi_int k) const { 588 return (*this)->repr_el(k); 589 } 590 get_diag(std::vector<casadi_int> & mapping) const591 Sparsity Sparsity::get_diag(std::vector<casadi_int>& mapping) const { 592 return (*this)->get_diag(mapping); 593 } 594 etree(bool ata) const595 std::vector<casadi_int> Sparsity::etree(bool ata) const { 596 vector<casadi_int> parent(size2()), w(size1() + size2()); 597 SparsityInternal::etree(*this, get_ptr(parent), get_ptr(w), ata); 598 return parent; 599 } 600 ldl(std::vector<casadi_int> & p,bool amd) const601 Sparsity Sparsity::ldl(std::vector<casadi_int>& p, bool amd) const { 602 casadi_assert(is_symmetric(), 603 "LDL factorization requires a symmetric matrix"); 604 // Recursive call if AMD 605 if (amd) { 606 // Get AMD reordering 607 p = this->amd(); 608 // Permute sparsity pattern 609 std::vector<casadi_int> tmp; 610 Sparsity Aperm = sub(p, p, tmp); 611 // Call recursively 612 return Aperm.ldl(tmp, false); 613 } 614 // Dimension 615 casadi_int n=size1(); 616 // Natural ordering 617 p = range(n); 618 // Work vector 619 std::vector<casadi_int> w(3*n); 620 // Elimination tree 621 std::vector<casadi_int> parent(n); 622 // Calculate colind in L (strictly lower entries only) 623 std::vector<casadi_int> L_colind(1+n); 624 SparsityInternal::ldl_colind(*this, get_ptr(parent), get_ptr(L_colind), get_ptr(w)); 625 // Get rows in L (strictly lower entries only) 626 std::vector<casadi_int> L_row(L_colind.back()); 627 SparsityInternal::ldl_row(*this, get_ptr(parent), get_ptr(L_colind), get_ptr(L_row), 628 get_ptr(w)); 629 // Sparsity of L^T 630 return Sparsity(n, n, L_colind, L_row, true).T(); 631 } 632 633 void Sparsity:: qr_sparse(Sparsity & V,Sparsity & R,std::vector<casadi_int> & prinv,std::vector<casadi_int> & pc,bool amd) const634 qr_sparse(Sparsity& V, Sparsity& R, std::vector<casadi_int>& prinv, 635 std::vector<casadi_int>& pc, bool amd) const { 636 // Dimensions 637 casadi_int size1=this->size1(), size2=this->size2(); 638 639 // Recursive call if AMD 640 if (amd) { 641 // Get AMD reordering 642 pc = mtimes(T(), *this).amd(); 643 // Permute sparsity pattern 644 std::vector<casadi_int> tmp; 645 Sparsity Aperm = sub(range(size1), pc, tmp); 646 // Call recursively 647 return Aperm.qr_sparse(V, R, prinv, tmp, false); 648 } 649 650 // No column permutation 651 pc = range(size2); 652 653 // Allocate memory 654 vector<casadi_int> leftmost(size1); 655 vector<casadi_int> parent(size2); 656 prinv.resize(size1 + size2); 657 vector<casadi_int> iw(size1 + 7*size2 + 1); 658 659 // Initialize QP solve 660 casadi_int nrow_ext, v_nnz, r_nnz; 661 SparsityInternal::qr_init(*this, T(), 662 get_ptr(leftmost), get_ptr(parent), get_ptr(prinv), 663 &nrow_ext, &v_nnz, &r_nnz, get_ptr(iw)); 664 665 // Calculate sparsities 666 vector<casadi_int> sp_v(2 + size2 + 1 + v_nnz); 667 vector<casadi_int> sp_r(2 + size2 + 1 + r_nnz); 668 SparsityInternal::qr_sparsities(*this, nrow_ext, get_ptr(sp_v), get_ptr(sp_r), 669 get_ptr(leftmost), get_ptr(parent), get_ptr(prinv), 670 get_ptr(iw)); 671 prinv.resize(nrow_ext); 672 V = compressed(sp_v, true); 673 R = compressed(sp_r, true); 674 } 675 dfs(casadi_int j,casadi_int top,std::vector<casadi_int> & xi,std::vector<casadi_int> & pstack,const std::vector<casadi_int> & pinv,std::vector<bool> & marked) const676 casadi_int Sparsity::dfs(casadi_int j, casadi_int top, std::vector<casadi_int>& xi, 677 std::vector<casadi_int>& pstack, 678 const std::vector<casadi_int>& pinv, 679 std::vector<bool>& marked) const { 680 return (*this)->dfs(j, top, xi, pstack, pinv, marked); 681 } 682 scc(std::vector<casadi_int> & index,std::vector<casadi_int> & offset) const683 casadi_int Sparsity::scc(std::vector<casadi_int>& index, std::vector<casadi_int>& offset) const { 684 return (*this)->scc(index, offset); 685 } 686 amd() const687 std::vector<casadi_int> Sparsity::amd() const { 688 return (*this)->amd(); 689 } 690 btf(std::vector<casadi_int> & rowperm,std::vector<casadi_int> & colperm,std::vector<casadi_int> & rowblock,std::vector<casadi_int> & colblock,std::vector<casadi_int> & coarse_rowblock,std::vector<casadi_int> & coarse_colblock) const691 casadi_int Sparsity::btf(std::vector<casadi_int>& rowperm, std::vector<casadi_int>& colperm, 692 std::vector<casadi_int>& rowblock, std::vector<casadi_int>& colblock, 693 std::vector<casadi_int>& coarse_rowblock, 694 std::vector<casadi_int>& coarse_colblock) const { 695 try { 696 return (*this)->btf(rowperm, colperm, rowblock, colblock, 697 coarse_rowblock, coarse_colblock); 698 } catch (exception &e) { 699 CASADI_THROW_ERROR("btf", e.what()); 700 } 701 } 702 spsolve(bvec_t * X,const bvec_t * B,bool tr) const703 void Sparsity::spsolve(bvec_t* X, const bvec_t* B, bool tr) const { 704 (*this)->spsolve(X, B, tr); 705 } 706 rowsSequential(bool strictly) const707 bool Sparsity::rowsSequential(bool strictly) const { 708 return (*this)->rowsSequential(strictly); 709 } 710 removeDuplicates(std::vector<casadi_int> & mapping)711 void Sparsity::removeDuplicates(std::vector<casadi_int>& mapping) { 712 *this = (*this)->_removeDuplicates(mapping); 713 } 714 find(bool ind1) const715 std::vector<casadi_int> Sparsity::find(bool ind1) const { 716 std::vector<casadi_int> loc; 717 find(loc, ind1); 718 return loc; 719 } 720 find(std::vector<casadi_int> & loc,bool ind1) const721 void Sparsity::find(std::vector<casadi_int>& loc, bool ind1) const { 722 (*this)->find(loc, ind1); 723 } 724 get_nz(std::vector<casadi_int> & indices) const725 void Sparsity::get_nz(std::vector<casadi_int>& indices) const { 726 (*this)->get_nz(indices); 727 } 728 uni_coloring(const Sparsity & AT,casadi_int cutoff) const729 Sparsity Sparsity::uni_coloring(const Sparsity& AT, casadi_int cutoff) const { 730 if (AT.is_null()) { 731 return (*this)->uni_coloring(T(), cutoff); 732 } else { 733 return (*this)->uni_coloring(AT, cutoff); 734 } 735 } 736 star_coloring(casadi_int ordering,casadi_int cutoff) const737 Sparsity Sparsity::star_coloring(casadi_int ordering, casadi_int cutoff) const { 738 return (*this)->star_coloring(ordering, cutoff); 739 } 740 star_coloring2(casadi_int ordering,casadi_int cutoff) const741 Sparsity Sparsity::star_coloring2(casadi_int ordering, casadi_int cutoff) const { 742 return (*this)->star_coloring2(ordering, cutoff); 743 } 744 largest_first() const745 std::vector<casadi_int> Sparsity::largest_first() const { 746 return (*this)->largest_first(); 747 } 748 pmult(const std::vector<casadi_int> & p,bool permute_rows,bool permute_columns,bool invert_permutation) const749 Sparsity Sparsity::pmult(const std::vector<casadi_int>& p, bool permute_rows, 750 bool permute_columns, bool invert_permutation) const { 751 return (*this)->pmult(p, permute_rows, permute_columns, invert_permutation); 752 } 753 spy_matlab(const std::string & mfile) const754 void Sparsity::spy_matlab(const std::string& mfile) const { 755 (*this)->spy_matlab(mfile); 756 } 757 export_code(const std::string & lang,std::ostream & stream,const Dict & options) const758 void Sparsity::export_code(const std::string& lang, std::ostream &stream, 759 const Dict& options) const { 760 (*this)->export_code(lang, stream, options); 761 } 762 spy(std::ostream & stream) const763 void Sparsity::spy(std::ostream &stream) const { 764 (*this)->spy(stream); 765 } 766 is_transpose(const Sparsity & y) const767 bool Sparsity::is_transpose(const Sparsity& y) const { 768 return (*this)->is_transpose(*y); 769 } 770 is_reshape(const Sparsity & y) const771 bool Sparsity::is_reshape(const Sparsity& y) const { 772 return (*this)->is_reshape(*y); 773 } 774 hash() const775 std::size_t Sparsity::hash() const { 776 return (*this)->hash(); 777 } 778 assign_cached(casadi_int nrow,casadi_int ncol,const std::vector<casadi_int> & colind,const std::vector<casadi_int> & row,bool order_rows)779 void Sparsity::assign_cached(casadi_int nrow, casadi_int ncol, 780 const std::vector<casadi_int>& colind, 781 const std::vector<casadi_int>& row, bool order_rows) { 782 casadi_assert_dev(colind.size()==ncol+1); 783 casadi_assert_dev(row.size()==colind.back()); 784 assign_cached(nrow, ncol, get_ptr(colind), get_ptr(row), order_rows); 785 } 786 assign_cached(casadi_int nrow,casadi_int ncol,const casadi_int * colind,const casadi_int * row,bool order_rows)787 void Sparsity::assign_cached(casadi_int nrow, casadi_int ncol, 788 const casadi_int* colind, const casadi_int* row, bool order_rows) { 789 // Scalars and empty patterns are handled separately 790 if (ncol==0 && nrow==0) { 791 // If empty 792 *this = getEmpty(); 793 return; 794 } else if (ncol==1 && nrow==1) { 795 if (colind[ncol]==0) { 796 // If sparse scalar 797 *this = getScalarSparse(); 798 return; 799 } else { 800 // If dense scalar 801 *this = getScalar(); 802 return; 803 } 804 } 805 806 // Make sure colind starts with zero 807 casadi_assert(colind[0]==0, 808 "Compressed Column Storage is not sane. " 809 "First element of colind must be zero."); 810 811 // Make sure colind is montone 812 for (casadi_int c=0; c<ncol; c++) { 813 casadi_assert(colind[c+1]>=colind[c], 814 "Compressed Column Storage is not sane. " 815 "colind must be monotone."); 816 } 817 818 // Check if rows correct and ordered without duplicates 819 bool rows_ordered = true; 820 for (casadi_int c=0; c<ncol; ++c) { 821 casadi_int last_r = -1; 822 for (casadi_int k=colind[c]; k<colind[c+1]; ++k) { 823 casadi_int r = row[k]; 824 // Make sure values are in within the [0,ncol) range 825 casadi_assert(r>=0 && r<nrow, 826 "Compressed Column Storage is not sane.\n" 827 "row[ " + str(k) + " == " + str(r) + " not in range " 828 "[0, " + str(nrow) + ")"); 829 // Check if ordered 830 if (r<=last_r) rows_ordered = false; 831 last_r = r; 832 } 833 } 834 835 // If unordered, need to order 836 if (!rows_ordered) { 837 casadi_assert(order_rows, 838 "Compressed Column Storage is not sane.\n" 839 "Row indices not strictly monotonically increasing for each " 840 "column and reordering is not enabled."); 841 // Number of nonzeros 842 casadi_int nnz = colind[ncol]; 843 // Get all columns 844 vector<casadi_int> col(nnz); 845 for (casadi_int c=0; c<ncol; ++c) { 846 for (casadi_int k=colind[c]; k<colind[c+1]; ++k) col[k] = c; 847 } 848 // Make sane via triplet format 849 *this = triplet(nrow, ncol, vector<casadi_int>(row, row+nnz), col); 850 return; 851 } 852 853 // Hash the pattern 854 std::size_t h = hash_sparsity(nrow, ncol, colind, row); 855 856 // Get a reference to the cache 857 CachingMap& cache = getCache(); 858 859 // Record the current number of buckets (for garbage collection below) 860 casadi_int bucket_count_before = cache.bucket_count(); 861 862 // WORKAROUND, functions do not appear to work when bucket_count==0 863 if (bucket_count_before>0) { 864 865 // Find the range of patterns equal to the key (normally only zero or one) 866 pair<CachingMap::iterator, CachingMap::iterator> eq = cache.equal_range(h); 867 868 // Loop over maching patterns 869 for (CachingMap::iterator i=eq.first; i!=eq.second; ++i) { 870 871 // Get a weak reference to the cached sparsity pattern 872 WeakRef& wref = i->second; 873 874 // Check if the pattern still exists 875 if (wref.alive()) { 876 877 // Get an owning reference to the cached pattern 878 Sparsity ref = shared_cast<Sparsity>(wref.shared()); 879 880 // Check if the pattern matches 881 if (ref.is_equal(nrow, ncol, colind, row)) { 882 883 // Found match! 884 own(ref.get()); 885 return; 886 887 } else { // There is a hash rowision (unlikely, but possible) 888 // Leave the pattern alone, continue to the next matching pattern 889 continue; 890 } 891 } else { 892 893 // Check if one of the other cache entries indeed has a matching sparsity 894 CachingMap::iterator j=i; 895 j++; // Start at the next matching key 896 for (; j!=eq.second; ++j) { 897 if (j->second.alive()) { 898 899 // Recover cached sparsity 900 Sparsity ref = shared_cast<Sparsity>(j->second.shared()); 901 902 // Match found if sparsity matches 903 if (ref.is_equal(nrow, ncol, colind, row)) { 904 own(ref.get()); 905 return; 906 } 907 } 908 } 909 910 // The cached entry has been deleted, create a new one 911 own(new SparsityInternal(nrow, ncol, colind, row)); 912 913 // Cache this pattern 914 wref = *this; 915 916 // Return 917 return; 918 } 919 } 920 } 921 922 // No matching sparsity pattern could be found, create a new one 923 own(new SparsityInternal(nrow, ncol, colind, row)); 924 925 // Cache this pattern 926 cache.insert(std::pair<std::size_t, WeakRef>(h, *this)); 927 928 // Garbage collection (currently only supported for unordered_multimap) 929 casadi_int bucket_count_after = cache.bucket_count(); 930 931 // We we increased the number of buckets, take time to garbage-collect deleted references 932 if (bucket_count_before!=bucket_count_after) { 933 CachingMap::const_iterator i=cache.begin(); 934 while (i!=cache.end()) { 935 if (!i->second.alive()) { 936 i = cache.erase(i); 937 } else { 938 i++; 939 } 940 } 941 } 942 } 943 tril(const Sparsity & x,bool includeDiagonal)944 Sparsity Sparsity::tril(const Sparsity& x, bool includeDiagonal) { 945 return x->_tril(includeDiagonal); 946 } 947 triu(const Sparsity & x,bool includeDiagonal)948 Sparsity Sparsity::triu(const Sparsity& x, bool includeDiagonal) { 949 return x->_triu(includeDiagonal); 950 } 951 get_lower() const952 std::vector<casadi_int> Sparsity::get_lower() const { 953 return (*this)->get_lower(); 954 } 955 get_upper() const956 std::vector<casadi_int> Sparsity::get_upper() const { 957 return (*this)->get_upper(); 958 } 959 960 hash_sparsity(casadi_int nrow,casadi_int ncol,const std::vector<casadi_int> & colind,const std::vector<casadi_int> & row)961 std::size_t hash_sparsity(casadi_int nrow, casadi_int ncol, const std::vector<casadi_int>& colind, 962 const std::vector<casadi_int>& row) { 963 return hash_sparsity(nrow, ncol, get_ptr(colind), get_ptr(row)); 964 } 965 hash_sparsity(casadi_int nrow,casadi_int ncol,const casadi_int * colind,const casadi_int * row)966 std::size_t hash_sparsity(casadi_int nrow, casadi_int ncol, 967 const casadi_int* colind, const casadi_int* row) { 968 // Condense the sparsity pattern to a single, deterministric number 969 std::size_t ret=0; 970 hash_combine(ret, nrow); 971 hash_combine(ret, ncol); 972 hash_combine(ret, colind, ncol+1); 973 hash_combine(ret, row, colind[ncol]); 974 return ret; 975 } 976 dense(casadi_int nrow,casadi_int ncol)977 Sparsity Sparsity::dense(casadi_int nrow, casadi_int ncol) { 978 casadi_assert_dev(nrow>=0); 979 casadi_assert_dev(ncol>=0); 980 // Column offset 981 std::vector<casadi_int> colind(ncol+1); 982 for (casadi_int cc=0; cc<ncol+1; ++cc) colind[cc] = cc*nrow; 983 984 // Row 985 std::vector<casadi_int> row(ncol*nrow); 986 for (casadi_int cc=0; cc<ncol; ++cc) 987 for (casadi_int rr=0; rr<nrow; ++rr) 988 row[rr+cc*nrow] = rr; 989 990 return Sparsity(nrow, ncol, colind, row); 991 } 992 upper(casadi_int n)993 Sparsity Sparsity::upper(casadi_int n) { 994 casadi_assert(n>=0, "Sparsity::upper expects a positive integer as argument"); 995 casadi_int nrow=n, ncol=n; 996 std::vector<casadi_int> colind, row; 997 colind.reserve(ncol+1); 998 row.reserve((n*(n+1))/2); 999 1000 // Loop over columns 1001 colind.push_back(0); 1002 for (casadi_int cc=0; cc<ncol; ++cc) { 1003 // Loop over rows for the upper triangular half 1004 for (casadi_int rr=0; rr<=cc; ++rr) { 1005 row.push_back(rr); 1006 } 1007 colind.push_back(row.size()); 1008 } 1009 1010 // Return the pattern 1011 return Sparsity(nrow, ncol, colind, row); 1012 } 1013 lower(casadi_int n)1014 Sparsity Sparsity::lower(casadi_int n) { 1015 casadi_assert(n>=0, "Sparsity::lower expects a positive integer as argument"); 1016 casadi_int nrow=n, ncol=n; 1017 std::vector<casadi_int> colind, row; 1018 colind.reserve(ncol+1); 1019 row.reserve((n*(n+1))/2); 1020 1021 // Loop over columns 1022 colind.push_back(0); 1023 for (casadi_int cc=0; cc<ncol; ++cc) { 1024 // Loop over rows for the lower triangular half 1025 for (casadi_int rr=cc; rr<nrow; ++rr) { 1026 row.push_back(rr); 1027 } 1028 colind.push_back(row.size()); 1029 } 1030 1031 // Return the pattern 1032 return Sparsity(nrow, ncol, colind, row); 1033 } 1034 band(casadi_int n,casadi_int p)1035 Sparsity Sparsity::band(casadi_int n, casadi_int p) { 1036 casadi_assert(n>=0, "Sparsity::band expects a positive integer as argument"); 1037 casadi_assert((p<0? -p : p)<n, 1038 "Sparsity::band: position of band schould be smaller then size argument"); 1039 1040 casadi_int nc = n-(p<0? -p : p); 1041 1042 std::vector< casadi_int > row(nc); 1043 1044 casadi_int offset = max(p, casadi_int(0)); 1045 for (casadi_int i=0;i<nc;i++) { 1046 row[i]=i+offset; 1047 } 1048 1049 std::vector< casadi_int > colind(n+1); 1050 1051 offset = min(p, casadi_int(0)); 1052 for (casadi_int i=0;i<n+1;i++) { 1053 colind[i]=max(min(i+offset, nc), casadi_int(0)); 1054 } 1055 1056 return Sparsity(n, n, colind, row); 1057 1058 } 1059 banded(casadi_int n,casadi_int p)1060 Sparsity Sparsity::banded(casadi_int n, casadi_int p) { 1061 // This is not an efficient implementation 1062 Sparsity ret = Sparsity(n, n); 1063 for (casadi_int i=-p;i<=p;++i) { 1064 ret = ret + Sparsity::band(n, i); 1065 } 1066 return ret; 1067 } 1068 unit(casadi_int n,casadi_int el)1069 Sparsity Sparsity::unit(casadi_int n, casadi_int el) { 1070 std::vector<casadi_int> row(1, el), colind(2); 1071 colind[0] = 0; 1072 colind[1] = 1; 1073 return Sparsity(n, 1, colind, row); 1074 } 1075 rowcol(const std::vector<casadi_int> & row,const std::vector<casadi_int> & col,casadi_int nrow,casadi_int ncol)1076 Sparsity Sparsity::rowcol(const std::vector<casadi_int>& row, const std::vector<casadi_int>& col, 1077 casadi_int nrow, casadi_int ncol) { 1078 std::vector<casadi_int> all_rows, all_cols; 1079 all_rows.reserve(row.size()*col.size()); 1080 all_cols.reserve(row.size()*col.size()); 1081 for (std::vector<casadi_int>::const_iterator c_it=col.begin(); c_it!=col.end(); ++c_it) { 1082 casadi_assert(*c_it>=0 && *c_it<ncol, "Sparsity::rowcol: Column index out of bounds"); 1083 for (std::vector<casadi_int>::const_iterator r_it=row.begin(); r_it!=row.end(); ++r_it) { 1084 casadi_assert(*r_it>=0 && *r_it<nrow, "Sparsity::rowcol: Row index out of bounds"); 1085 all_rows.push_back(*r_it); 1086 all_cols.push_back(*c_it); 1087 } 1088 } 1089 return Sparsity::triplet(nrow, ncol, all_rows, all_cols); 1090 } 1091 triplet(casadi_int nrow,casadi_int ncol,const std::vector<casadi_int> & row,const std::vector<casadi_int> & col,std::vector<casadi_int> & mapping,bool invert_mapping)1092 Sparsity Sparsity::triplet(casadi_int nrow, casadi_int ncol, const std::vector<casadi_int>& row, 1093 const std::vector<casadi_int>& col, std::vector<casadi_int>& mapping, 1094 bool invert_mapping) { 1095 // Assert dimensions 1096 casadi_assert_dev(nrow>=0); 1097 casadi_assert_dev(ncol>=0); 1098 casadi_assert(col.size()==row.size(), "inconsistent lengths"); 1099 1100 // Create the return sparsity pattern and access vectors 1101 std::vector<casadi_int> r_colind(ncol+1, 0); 1102 std::vector<casadi_int> r_row; 1103 r_row.reserve(row.size()); 1104 1105 // Consistency check and check if elements are already perfectly ordered with no duplicates 1106 casadi_int last_col=-1, last_row=-1; 1107 bool perfectly_ordered=true; 1108 for (casadi_int k=0; k<col.size(); ++k) { 1109 // Consistency check 1110 casadi_assert(col[k]>=0 && col[k]<ncol, 1111 "Column index (" + str(col[k]) + ") out of bounds [0," + str(ncol) + "["); 1112 casadi_assert(row[k]>=0 && row[k]<nrow, 1113 "Row index out of bounds (" + str(row[k]) + ") out of bounds [0," + str(nrow) + "["); 1114 1115 // Check if ordering is already perfect 1116 perfectly_ordered = perfectly_ordered && (col[k]<last_col || 1117 (col[k]==last_col && row[k]<=last_row)); 1118 last_col = col[k]; 1119 last_row = row[k]; 1120 } 1121 1122 // Quick return if perfectly ordered 1123 if (perfectly_ordered) { 1124 // Save rows 1125 r_row.resize(row.size()); 1126 copy(row.begin(), row.end(), r_row.begin()); 1127 1128 // Find offset index 1129 casadi_int el=0; 1130 for (casadi_int i=0; i<ncol; ++i) { 1131 while (el<col.size() && col[el]==i) el++; 1132 r_colind[i+1] = el; 1133 } 1134 1135 // Identity mapping 1136 mapping.resize(row.size()); 1137 for (casadi_int k=0; k<row.size(); ++k) mapping[k] = k; 1138 1139 // Quick return 1140 return Sparsity(nrow, ncol, r_colind, r_row); 1141 } 1142 1143 // Reuse data 1144 std::vector<casadi_int>& mapping1 = invert_mapping ? r_row : mapping; 1145 std::vector<casadi_int>& mapping2 = invert_mapping ? mapping : r_row; 1146 1147 // Make sure that enough memory is allocated to use as a work vector 1148 mapping1.reserve(std::max(nrow+1, static_cast<casadi_int>(col.size()))); 1149 1150 // Number of elements in each row 1151 std::vector<casadi_int>& rowcount = mapping1; // reuse memory 1152 rowcount.resize(nrow+1); 1153 fill(rowcount.begin(), rowcount.end(), 0); 1154 for (std::vector<casadi_int>::const_iterator it=row.begin(); it!=row.end(); ++it) { 1155 rowcount[*it+1]++; 1156 } 1157 1158 // Cumsum to get index offset for each row 1159 for (casadi_int i=0; i<nrow; ++i) { 1160 rowcount[i+1] += rowcount[i]; 1161 } 1162 1163 // New row for each old row 1164 mapping2.resize(row.size()); 1165 for (casadi_int k=0; k<row.size(); ++k) { 1166 mapping2[rowcount[row[k]]++] = k; 1167 } 1168 1169 // Number of elements in each col 1170 // reuse memory, r_colind is already the right size 1171 std::vector<casadi_int>& colcount = r_colind; 1172 // and is filled with zeros 1173 for (std::vector<casadi_int>::const_iterator it=mapping2.begin(); it!=mapping2.end(); ++it) { 1174 colcount[col[*it]+1]++; 1175 } 1176 1177 // Cumsum to get index offset for each col 1178 for (casadi_int i=0; i<ncol; ++i) { 1179 colcount[i+1] += colcount[i]; 1180 } 1181 1182 // New col for each old col 1183 mapping1.resize(col.size()); 1184 for (std::vector<casadi_int>::const_iterator it=mapping2.begin(); it!=mapping2.end(); ++it) { 1185 mapping1[colcount[col[*it]]++] = *it; 1186 } 1187 1188 // Current element in the return matrix 1189 casadi_int r_el = 0; 1190 r_row.resize(col.size()); 1191 1192 // Current nonzero 1193 std::vector<casadi_int>::const_iterator it=mapping1.begin(); 1194 1195 // Loop over columns 1196 r_colind[0] = 0; 1197 for (casadi_int i=0; i<ncol; ++i) { 1198 1199 // Previous row (to detect duplicates) 1200 casadi_int j_prev = -1; 1201 1202 // Loop over nonzero elements of the col 1203 while (it!=mapping1.end() && col[*it]==i) { 1204 1205 // Get the element 1206 casadi_int el = *it; 1207 it++; 1208 1209 // Get the row 1210 casadi_int j = row[el]; 1211 1212 // If not a duplicate, save to return matrix 1213 if (j!=j_prev) 1214 r_row[r_el++] = j; 1215 1216 if (invert_mapping) { 1217 // Save to the inverse mapping 1218 mapping2[el] = r_el-1; 1219 } else { 1220 // If not a duplicate, save to the mapping vector 1221 if (j!=j_prev) 1222 mapping1[r_el-1] = el; 1223 } 1224 1225 // Save row 1226 j_prev = j; 1227 } 1228 1229 // Update col offset 1230 r_colind[i+1] = r_el; 1231 } 1232 1233 // Resize the row vector 1234 r_row.resize(r_el); 1235 1236 // Resize mapping matrix 1237 if (!invert_mapping) { 1238 mapping1.resize(r_el); 1239 } 1240 1241 return Sparsity(nrow, ncol, r_colind, r_row); 1242 } 1243 triplet(casadi_int nrow,casadi_int ncol,const std::vector<casadi_int> & row,const std::vector<casadi_int> & col)1244 Sparsity Sparsity::triplet(casadi_int nrow, casadi_int ncol, const std::vector<casadi_int>& row, 1245 const std::vector<casadi_int>& col) { 1246 std::vector<casadi_int> mapping; 1247 return Sparsity::triplet(nrow, ncol, row, col, mapping, false); 1248 } 1249 nonzeros(casadi_int nrow,casadi_int ncol,const std::vector<casadi_int> & nz,bool ind1)1250 Sparsity Sparsity::nonzeros(casadi_int nrow, casadi_int ncol, 1251 const std::vector<casadi_int>& nz, bool ind1) { 1252 casadi_assert(nrow>0, "nrow must be >0."); 1253 std::vector<casadi_int> row(nz.size()); 1254 std::vector<casadi_int> col(nz.size()); 1255 for (casadi_int i=0;i<nz.size();++i) { 1256 casadi_int k = nz[i]; 1257 k-= ind1; 1258 row[i] = k % nrow; 1259 col[i] = k / nrow; 1260 } 1261 return triplet(nrow, ncol, row, col); 1262 } 1263 is_singular() const1264 bool Sparsity::is_singular() const { 1265 casadi_assert(is_square(), 1266 "is_singular: only defined for square matrices, but got " + dim()); 1267 return sprank(*this)!=size2(); 1268 } 1269 compress() const1270 std::vector<casadi_int> Sparsity::compress() const { 1271 return (*this)->sp(); 1272 } 1273 operator const std::vector<casadi_int>&() const1274 Sparsity::operator const std::vector<casadi_int>&() const { 1275 return (*this)->sp(); 1276 } 1277 operator SparsityStruct() const1278 Sparsity::operator SparsityStruct() const { 1279 const casadi_int* sp = *this; 1280 casadi_int nrow = sp[0], ncol = sp[1]; 1281 const casadi_int* colind = sp+2, *row = sp+2+ncol+1; 1282 return SparsityStruct{nrow, ncol, colind, row}; 1283 } 1284 compressed(const std::vector<casadi_int> & v,bool order_rows)1285 Sparsity Sparsity::compressed(const std::vector<casadi_int>& v, bool order_rows) { 1286 // Check consistency 1287 casadi_assert_dev(v.size() >= 2); 1288 casadi_int nrow = v[0]; 1289 casadi_int ncol = v[1]; 1290 casadi_assert_dev(v.size() >= 2 + ncol+1); 1291 casadi_int nnz = v[2 + ncol]; 1292 bool dense = v.size() == 2 + ncol+1 && nrow*ncol==nnz; 1293 bool sparse = v.size() == 2 + ncol+1 + nnz; 1294 casadi_assert_dev(dense || sparse); 1295 1296 // Call array version 1297 return compressed(&v.front(), order_rows); 1298 } 1299 compressed(const casadi_int * v,bool order_rows)1300 Sparsity Sparsity::compressed(const casadi_int* v, bool order_rows) { 1301 casadi_assert_dev(v!=nullptr); 1302 1303 // Get sparsity pattern 1304 casadi_int nrow = v[0]; 1305 casadi_int ncol = v[1]; 1306 const casadi_int *colind = v+2; 1307 if (colind[0]==1) { 1308 // Dense matrix 1309 return Sparsity::dense(nrow, ncol); 1310 } 1311 casadi_int nnz = colind[ncol]; 1312 if (nrow*ncol == nnz) { 1313 // Dense matrix 1314 return Sparsity::dense(nrow, ncol); 1315 } else { 1316 // Sparse matrix 1317 const casadi_int *row = v + 2 + ncol+1; 1318 return Sparsity(nrow, ncol, 1319 vector<casadi_int>(colind, colind+ncol+1), 1320 vector<casadi_int>(row, row+nnz), order_rows); 1321 } 1322 } 1323 bw_upper() const1324 casadi_int Sparsity::bw_upper() const { 1325 return (*this)->bw_upper(); 1326 } 1327 bw_lower() const1328 casadi_int Sparsity::bw_lower() const { 1329 return (*this)->bw_lower(); 1330 } 1331 horzcat(const std::vector<Sparsity> & sp)1332 Sparsity Sparsity::horzcat(const std::vector<Sparsity> & sp) { 1333 // Quick return if possible 1334 if (sp.empty()) return Sparsity(0, 0); 1335 if (sp.size()==1) return sp.front(); 1336 1337 // Count total nnz 1338 casadi_int nnz_total = 0; 1339 for (casadi_int i=0; i<sp.size(); ++i) nnz_total += sp[i].nnz(); 1340 1341 // Construct from vectors (triplet format) 1342 vector<casadi_int> ret_row, ret_col; 1343 ret_row.reserve(nnz_total); 1344 ret_col.reserve(nnz_total); 1345 casadi_int ret_ncol = 0; 1346 casadi_int ret_nrow = 0; 1347 for (casadi_int i=0; i<sp.size() && ret_nrow==0; ++i) 1348 ret_nrow = sp[i].size1(); 1349 1350 // Append all patterns 1351 for (vector<Sparsity>::const_iterator i=sp.begin(); i!=sp.end(); ++i) { 1352 // Get sparsity pattern 1353 casadi_int sp_nrow = i->size1(); 1354 casadi_int sp_ncol = i->size2(); 1355 const casadi_int* sp_colind = i->colind(); 1356 const casadi_int* sp_row = i->row(); 1357 casadi_assert(sp_nrow==ret_nrow || sp_nrow==0, 1358 "Sparsity::horzcat: Mismatching number of rows"); 1359 1360 // Add entries to pattern 1361 for (casadi_int cc=0; cc<sp_ncol; ++cc) { 1362 for (casadi_int k=sp_colind[cc]; k<sp_colind[cc+1]; ++k) { 1363 ret_row.push_back(sp_row[k]); 1364 ret_col.push_back(cc + ret_ncol); 1365 } 1366 } 1367 1368 // Update offset 1369 ret_ncol += sp_ncol; 1370 } 1371 return Sparsity::triplet(ret_nrow, ret_ncol, ret_row, ret_col); 1372 } 1373 kron(const Sparsity & a,const Sparsity & b)1374 Sparsity Sparsity::kron(const Sparsity& a, const Sparsity& b) { 1375 casadi_int a_ncol = a.size2(); 1376 casadi_int b_ncol = b.size2(); 1377 casadi_int a_nrow = a.size1(); 1378 casadi_int b_nrow = b.size1(); 1379 if (a.is_dense() && b.is_dense()) return Sparsity::dense(a_nrow*b_nrow, a_ncol*b_ncol); 1380 1381 const casadi_int* a_colind = a.colind(); 1382 const casadi_int* a_row = a.row(); 1383 const casadi_int* b_colind = b.colind(); 1384 const casadi_int* b_row = b.row(); 1385 1386 std::vector<casadi_int> r_colind(a_ncol*b_ncol+1, 0); 1387 std::vector<casadi_int> r_row(a.nnz()*b.nnz()); 1388 1389 casadi_int* r_colind_ptr = get_ptr(r_colind); 1390 casadi_int* r_row_ptr = get_ptr(r_row); 1391 1392 casadi_int i=0; 1393 casadi_int j=0; 1394 // Loop over the columns 1395 for (casadi_int a_cc=0; a_cc<a_ncol; ++a_cc) { 1396 casadi_int a_start = a_colind[a_cc]; 1397 casadi_int a_stop = a_colind[a_cc+1]; 1398 // Loop over the columns 1399 for (casadi_int b_cc=0; b_cc<b_ncol; ++b_cc) { 1400 casadi_int b_start = b_colind[b_cc]; 1401 casadi_int b_stop = b_colind[b_cc+1]; 1402 // Loop over existing nonzeros 1403 for (casadi_int a_el=a_start; a_el<a_stop; ++a_el) { 1404 casadi_int a_r = a_row[a_el]; 1405 // Loop over existing nonzeros 1406 for (casadi_int b_el=b_start; b_el<b_stop; ++b_el) { 1407 casadi_int b_r = b_row[b_el]; 1408 r_row_ptr[i++] = a_r*b_nrow+b_r; 1409 } 1410 } 1411 j+=1; 1412 r_colind_ptr[j] = r_colind_ptr[j-1] + (b_stop-b_start)*(a_stop-a_start); 1413 } 1414 } 1415 return Sparsity(a_nrow*b_nrow, a_ncol*b_ncol, r_colind, r_row); 1416 } 1417 vertcat(const std::vector<Sparsity> & sp)1418 Sparsity Sparsity::vertcat(const std::vector<Sparsity> & sp) { 1419 // Quick return if possible 1420 if (sp.empty()) return Sparsity(0, 0); 1421 if (sp.size()==1) return sp.front(); 1422 1423 // Count total nnz 1424 casadi_int nnz_total = 0; 1425 for (casadi_int i=0; i<sp.size(); ++i) nnz_total += sp[i].nnz(); 1426 1427 // Construct from vectors (triplet format) 1428 vector<casadi_int> ret_row, ret_col; 1429 ret_row.reserve(nnz_total); 1430 ret_col.reserve(nnz_total); 1431 casadi_int ret_nrow = 0; 1432 casadi_int ret_ncol = 0; 1433 for (casadi_int i=0; i<sp.size() && ret_ncol==0; ++i) 1434 ret_ncol = sp[i].size2(); 1435 1436 // Append all patterns 1437 for (vector<Sparsity>::const_iterator i=sp.begin(); i!=sp.end(); ++i) { 1438 // Get sparsity pattern 1439 casadi_int sp_nrow = i->size1(); 1440 casadi_int sp_ncol = i->size2(); 1441 const casadi_int* sp_colind = i->colind(); 1442 const casadi_int* sp_row = i->row(); 1443 casadi_assert(sp_ncol==ret_ncol || sp_ncol==0, 1444 "Sparsity::vertcat: Mismatching number of columns"); 1445 1446 // Add entries to pattern 1447 for (casadi_int cc=0; cc<sp_ncol; ++cc) { 1448 for (casadi_int k=sp_colind[cc]; k<sp_colind[cc+1]; ++k) { 1449 ret_row.push_back(sp_row[k] + ret_nrow); 1450 ret_col.push_back(cc); 1451 } 1452 } 1453 1454 // Update offset 1455 ret_nrow += sp_nrow; 1456 } 1457 return Sparsity::triplet(ret_nrow, ret_ncol, ret_row, ret_col); 1458 } 1459 diagcat(const std::vector<Sparsity> & v)1460 Sparsity Sparsity::diagcat(const std::vector< Sparsity > &v) { 1461 casadi_int n = 0; 1462 casadi_int m = 0; 1463 1464 std::vector<casadi_int> colind(1, 0); 1465 std::vector<casadi_int> row; 1466 1467 casadi_int nz = 0; 1468 for (casadi_int i=0;i<v.size();++i) { 1469 const casadi_int* colind_ = v[i].colind(); 1470 casadi_int ncol = v[i].size2(); 1471 const casadi_int* row_ = v[i].row(); 1472 casadi_int sz = v[i].nnz(); 1473 for (casadi_int k=1; k<ncol+1; ++k) { 1474 colind.push_back(colind_[k]+nz); 1475 } 1476 for (casadi_int k=0; k<sz; ++k) { 1477 row.push_back(row_[k]+m); 1478 } 1479 n+= v[i].size2(); 1480 m+= v[i].size1(); 1481 nz+= v[i].nnz(); 1482 } 1483 1484 return Sparsity(m, n, colind, row); 1485 } 1486 horzsplit(const Sparsity & x,const std::vector<casadi_int> & offset)1487 std::vector<Sparsity> Sparsity::horzsplit(const Sparsity& x, 1488 const std::vector<casadi_int>& offset) { 1489 // Consistency check 1490 casadi_assert_dev(!offset.empty()); 1491 casadi_assert_dev(offset.front()==0); 1492 casadi_assert(offset.back()==x.size2(), 1493 "horzsplit(Sparsity, std::vector<casadi_int>): Last elements of offset " 1494 "(" + str(offset.back()) + ") must equal the number of columns " 1495 "(" + str(x.size2()) + ")"); 1496 casadi_assert_dev(is_monotone(offset)); 1497 1498 // Number of outputs 1499 casadi_int n = offset.size()-1; 1500 1501 // Get the sparsity of the input 1502 const casadi_int* colind_x = x.colind(); 1503 const casadi_int* row_x = x.row(); 1504 1505 // Allocate result 1506 std::vector<Sparsity> ret; 1507 ret.reserve(n); 1508 1509 // Sparsity pattern as CCS vectors 1510 vector<casadi_int> colind, row; 1511 casadi_int ncol, nrow = x.size1(); 1512 1513 // Get the sparsity patterns of the outputs 1514 for (casadi_int i=0; i<n; ++i) { 1515 casadi_int first_col = offset[i]; 1516 casadi_int last_col = offset[i+1]; 1517 ncol = last_col - first_col; 1518 1519 // Construct the sparsity pattern 1520 colind.resize(ncol+1); 1521 copy(colind_x+first_col, colind_x+last_col+1, colind.begin()); 1522 for (vector<casadi_int>::iterator it=colind.begin()+1; it!=colind.end(); ++it) 1523 *it -= colind[0]; 1524 colind[0] = 0; 1525 row.resize(colind.back()); 1526 copy(row_x+colind_x[first_col], row_x+colind_x[last_col], row.begin()); 1527 1528 // Append to the list 1529 ret.push_back(Sparsity(nrow, ncol, colind, row)); 1530 } 1531 1532 // Return (RVO) 1533 return ret; 1534 } 1535 vertsplit(const Sparsity & x,const std::vector<casadi_int> & offset)1536 std::vector<Sparsity> Sparsity::vertsplit(const Sparsity& x, 1537 const std::vector<casadi_int>& offset) { 1538 std::vector<Sparsity> ret = horzsplit(x.T(), offset); 1539 for (std::vector<Sparsity>::iterator it=ret.begin(); it!=ret.end(); ++it) { 1540 *it = it->T(); 1541 } 1542 return ret; 1543 } 1544 blockcat(const std::vector<std::vector<Sparsity>> & v)1545 Sparsity Sparsity::blockcat(const std::vector< std::vector< Sparsity > > &v) { 1546 std::vector< Sparsity > ret; 1547 for (casadi_int i=0; i<v.size(); ++i) 1548 ret.push_back(horzcat(v[i])); 1549 return vertcat(ret); 1550 } 1551 diagsplit(const Sparsity & x,const std::vector<casadi_int> & offset1,const std::vector<casadi_int> & offset2)1552 std::vector<Sparsity> Sparsity::diagsplit(const Sparsity& x, 1553 const std::vector<casadi_int>& offset1, 1554 const std::vector<casadi_int>& offset2) { 1555 // Consistency check 1556 casadi_assert_dev(!offset1.empty()); 1557 casadi_assert_dev(offset1.front()==0); 1558 casadi_assert(offset1.back()==x.size1(), 1559 "diagsplit(Sparsity, offset1, offset2): Last elements of offset1 " 1560 "(" + str(offset1.back()) + ") must equal the number of rows " 1561 "(" + str(x.size1()) + ")"); 1562 casadi_assert(offset2.back()==x.size2(), 1563 "diagsplit(Sparsity, offset1, offset2): Last elements of offset2 " 1564 "(" + str(offset2.back()) + ") must equal the number of rows " 1565 "(" + str(x.size2()) + ")"); 1566 casadi_assert_dev(is_monotone(offset1)); 1567 casadi_assert_dev(is_monotone(offset2)); 1568 casadi_assert_dev(offset1.size()==offset2.size()); 1569 1570 // Number of outputs 1571 casadi_int n = offset1.size()-1; 1572 1573 // Return value 1574 std::vector<Sparsity> ret; 1575 1576 // Caveat: this is a very silly implementation 1577 IM x2 = IM::zeros(x); 1578 1579 for (casadi_int i=0; i<n; ++i) { 1580 ret.push_back(x2(Slice(offset1[i], offset1[i+1]), 1581 Slice(offset2[i], offset2[i+1])).sparsity()); 1582 } 1583 1584 return ret; 1585 } 1586 sum2(const Sparsity & x)1587 Sparsity Sparsity::sum2(const Sparsity &x) { 1588 return mtimes(x, Sparsity::dense(x.size2(), 1)); 1589 1590 } sum1(const Sparsity & x)1591 Sparsity Sparsity::sum1(const Sparsity &x) { 1592 return mtimes(Sparsity::dense(1, x.size1()), x); 1593 } 1594 sprank(const Sparsity & x)1595 casadi_int Sparsity::sprank(const Sparsity& x) { 1596 std::vector<casadi_int> rowperm, colperm, rowblock, colblock, coarse_rowblock, coarse_colblock; 1597 x.btf(rowperm, colperm, rowblock, colblock, coarse_rowblock, coarse_colblock); 1598 return coarse_colblock.at(3); 1599 } 1600 operator const casadi_int*() const1601 Sparsity::operator const casadi_int*() const { 1602 return &(*this)->sp().front(); 1603 } 1604 norm_0_mul(const Sparsity & x,const Sparsity & A)1605 casadi_int Sparsity::norm_0_mul(const Sparsity& x, const Sparsity& A) { 1606 // Implementation borrowed from Scipy's sparsetools/csr.h 1607 casadi_assert(A.size1()==x.size2(), "Dimension error. Got " + x.dim() 1608 + " times " + A.dim() + "."); 1609 1610 casadi_int n_row = A.size2(); 1611 casadi_int n_col = x.size1(); 1612 1613 // Allocate work vectors 1614 std::vector<bool> Bwork(n_col); 1615 std::vector<casadi_int> Iwork(n_row+1+n_col); 1616 1617 const casadi_int* Aj = A.row(); 1618 const casadi_int* Ap = A.colind(); 1619 const casadi_int* Bj = x.row(); 1620 const casadi_int* Bp = x.colind(); 1621 casadi_int *Cp = get_ptr(Iwork); 1622 casadi_int *mask = Cp+n_row+1; 1623 1624 // Pass 1 1625 // method that uses O(n) temp storage 1626 std::fill(mask, mask+n_col, -1); 1627 1628 Cp[0] = 0; 1629 casadi_int nnz = 0; 1630 for (casadi_int i = 0; i < n_row; i++) { 1631 casadi_int row_nnz = 0; 1632 for (casadi_int jj = Ap[i]; jj < Ap[i+1]; jj++) { 1633 casadi_int j = Aj[jj]; 1634 for (casadi_int kk = Bp[j]; kk < Bp[j+1]; kk++) { 1635 casadi_int k = Bj[kk]; 1636 if (mask[k] != i) { 1637 mask[k] = i; 1638 row_nnz++; 1639 } 1640 } 1641 } 1642 casadi_int next_nnz = nnz + row_nnz; 1643 nnz = next_nnz; 1644 Cp[i+1] = nnz; 1645 } 1646 1647 // Pass 2 1648 casadi_int *next = get_ptr(Iwork) + n_row+1; 1649 std::fill(next, next+n_col, -1); 1650 std::vector<bool> & sums = Bwork; 1651 std::fill(sums.begin(), sums.end(), false); 1652 nnz = 0; 1653 Cp[0] = 0; 1654 for (casadi_int i = 0; i < n_row; i++) { 1655 casadi_int head = -2; 1656 casadi_int length = 0; 1657 casadi_int jj_start = Ap[i]; 1658 casadi_int jj_end = Ap[i+1]; 1659 for (casadi_int jj = jj_start; jj < jj_end; jj++) { 1660 casadi_int j = Aj[jj]; 1661 casadi_int kk_start = Bp[j]; 1662 casadi_int kk_end = Bp[j+1]; 1663 for (casadi_int kk = kk_start; kk < kk_end; kk++) { 1664 casadi_int k = Bj[kk]; 1665 sums[k] = true; 1666 if (next[k] == -1) { 1667 next[k] = head; 1668 head = k; 1669 length++; 1670 } 1671 } 1672 } 1673 for (casadi_int jj = 0; jj < length; jj++) { 1674 if (sums[head]) { 1675 nnz++; 1676 } 1677 casadi_int temp = head; 1678 head = next[head]; 1679 next[temp] = -1; //clear arrays 1680 sums[temp] = 0; 1681 } 1682 Cp[i+1] = nnz; 1683 } 1684 return nnz; 1685 } 1686 mul_sparsityF(const bvec_t * x,const Sparsity & x_sp,const bvec_t * y,const Sparsity & y_sp,bvec_t * z,const Sparsity & z_sp,bvec_t * w)1687 void Sparsity::mul_sparsityF(const bvec_t* x, const Sparsity& x_sp, 1688 const bvec_t* y, const Sparsity& y_sp, 1689 bvec_t* z, const Sparsity& z_sp, 1690 bvec_t* w) { 1691 // Assert dimensions 1692 casadi_assert(z_sp.size1()==x_sp.size1() && x_sp.size2()==y_sp.size1() 1693 && y_sp.size2()==z_sp.size2(), 1694 "Dimension error. Got x=" + x_sp.dim() + ", y=" + y_sp.dim() 1695 + " and z=" + z_sp.dim() + "."); 1696 1697 // Direct access to the arrays 1698 const casadi_int* y_colind = y_sp.colind(); 1699 const casadi_int* y_row = y_sp.row(); 1700 const casadi_int* x_colind = x_sp.colind(); 1701 const casadi_int* x_row = x_sp.row(); 1702 const casadi_int* z_colind = z_sp.colind(); 1703 const casadi_int* z_row = z_sp.row(); 1704 1705 // Loop over the columns of y and z 1706 casadi_int ncol = z_sp.size2(); 1707 for (casadi_int cc=0; cc<ncol; ++cc) { 1708 // Get the dense column of z 1709 for (casadi_int kk=z_colind[cc]; kk<z_colind[cc+1]; ++kk) { 1710 w[z_row[kk]] = z[kk]; 1711 } 1712 1713 // Loop over the nonzeros of y 1714 for (casadi_int kk=y_colind[cc]; kk<y_colind[cc+1]; ++kk) { 1715 casadi_int rr = y_row[kk]; 1716 1717 // Loop over corresponding columns of x 1718 bvec_t yy = y[kk]; 1719 for (casadi_int kk1=x_colind[rr]; kk1<x_colind[rr+1]; ++kk1) { 1720 w[x_row[kk1]] |= x[kk1] | yy; 1721 } 1722 } 1723 1724 // Get the sparse column of z 1725 for (casadi_int kk=z_colind[cc]; kk<z_colind[cc+1]; ++kk) { 1726 z[kk] = w[z_row[kk]]; 1727 } 1728 } 1729 } 1730 mul_sparsityR(bvec_t * x,const Sparsity & x_sp,bvec_t * y,const Sparsity & y_sp,bvec_t * z,const Sparsity & z_sp,bvec_t * w)1731 void Sparsity::mul_sparsityR(bvec_t* x, const Sparsity& x_sp, 1732 bvec_t* y, const Sparsity& y_sp, 1733 bvec_t* z, const Sparsity& z_sp, 1734 bvec_t* w) { 1735 // Assert dimensions 1736 casadi_assert(z_sp.size1()==x_sp.size1() && x_sp.size2()==y_sp.size1() 1737 && y_sp.size2()==z_sp.size2(), 1738 "Dimension error. Got x=" + x_sp.dim() + ", y=" + y_sp.dim() 1739 + " and z=" + z_sp.dim() + "."); 1740 1741 // Direct access to the arrays 1742 const casadi_int* y_colind = y_sp.colind(); 1743 const casadi_int* y_row = y_sp.row(); 1744 const casadi_int* x_colind = x_sp.colind(); 1745 const casadi_int* x_row = x_sp.row(); 1746 const casadi_int* z_colind = z_sp.colind(); 1747 const casadi_int* z_row = z_sp.row(); 1748 1749 // Clear residual work vector data from preceding operations (not necessary 1750 // for data from this method if conditional clear code is made unconditional 1751 // in loop) 1752 casadi_int nrow = z_sp.size1(); 1753 casadi_fill(w, nrow, static_cast<bvec_t>(0)); 1754 1755 // Loop over the columns of y and z 1756 casadi_int ncol = z_sp.size2(); 1757 for (casadi_int cc=0; cc<ncol; ++cc) { 1758 // Get the dense column of z 1759 for (casadi_int kk=z_colind[cc]; kk<z_colind[cc+1]; ++kk) { 1760 w[z_row[kk]] = z[kk]; 1761 } 1762 1763 // Loop over the nonzeros of y 1764 for (casadi_int kk=y_colind[cc]; kk<y_colind[cc+1]; ++kk) { 1765 casadi_int rr = y_row[kk]; 1766 1767 // Loop over corresponding columns of x 1768 bvec_t yy = 0; 1769 for (casadi_int kk1=x_colind[rr]; kk1<x_colind[rr+1]; ++kk1) { 1770 yy |= w[x_row[kk1]]; 1771 x[kk1] |= w[x_row[kk1]]; 1772 } 1773 y[kk] |= yy; 1774 } 1775 1776 // Get the sparse column of z, clear work vector for next column 1777 for (casadi_int kk=z_colind[cc]; kk<z_colind[cc+1]; ++kk) { 1778 z[kk] = w[z_row[kk]]; 1779 w[z_row[kk]] = 0; 1780 } 1781 } 1782 } 1783 info() const1784 Dict Sparsity::info() const { 1785 if (is_null()) return Dict(); 1786 return {{"nrow", size1()}, {"ncol", size2()}, {"colind", get_colind()}, {"row", get_row()}}; 1787 } 1788 1789 std::set<std::string> Sparsity::file_formats = {"mtx"}; 1790 file_format(const std::string & filename,const std::string & format_hint,const std::set<std::string> & file_formats)1791 std::string Sparsity::file_format(const std::string& filename, 1792 const std::string& format_hint, const std::set<std::string>& file_formats) { 1793 if (format_hint.empty()) { 1794 std::string extension = filename.substr(filename.rfind(".")+1); 1795 auto it = file_formats.find(extension); 1796 casadi_assert(it!=file_formats.end(), 1797 "Extension '" + extension + "' not recognised. " 1798 "Valid options: " + str(file_formats) + "."); 1799 return extension; 1800 } else { 1801 auto it = file_formats.find(format_hint); 1802 casadi_assert(it!=file_formats.end(), 1803 "File format hint '" + format_hint + "' not recognised. " 1804 "Valid options: " + str(file_formats) + "."); 1805 return format_hint; 1806 } 1807 1808 } to_file(const std::string & filename,const std::string & format_hint) const1809 void Sparsity::to_file(const std::string& filename, const std::string& format_hint) const { 1810 std::string format = file_format(filename, format_hint, file_formats); 1811 std::ofstream out(filename); 1812 if (format=="mtx") { 1813 out << std::scientific << std::setprecision(std::numeric_limits<double>::digits10 + 1); 1814 out << "%%MatrixMarket matrix coordinate pattern general" << std::endl; 1815 out << size1() << " " << size2() << " " << nnz() << std::endl; 1816 std::vector<casadi_int> row = get_row(); 1817 std::vector<casadi_int> col = get_col(); 1818 1819 for (casadi_int k=0;k<row.size();++k) { 1820 out << row[k]+1 << " " << col[k]+1 << std::endl; 1821 } 1822 } else { 1823 casadi_error("Unknown format '" + format + "'"); 1824 } 1825 } 1826 from_file(const std::string & filename,const std::string & format_hint)1827 Sparsity Sparsity::from_file(const std::string& filename, const std::string& format_hint) { 1828 std::string format = file_format(filename, format_hint, file_formats); 1829 std::ifstream in(filename); 1830 casadi_assert(in.good(), "Could not open '" + filename + "'."); 1831 if (format=="mtx") { 1832 std::string line; 1833 std::vector<casadi_int> row, col; 1834 casadi_int size1, size2, nnz; 1835 int line_num = 0; 1836 while (std::getline(in, line)) { 1837 if (line_num==0) { 1838 casadi_assert(line=="%%MatrixMarket matrix coordinate pattern general", "Wrong header"); 1839 line_num = 1; 1840 } else if (line_num==1) { 1841 std::stringstream stream(line); 1842 stream >> size1; 1843 stream >> size2; 1844 stream >> nnz; 1845 row.reserve(nnz); 1846 col.reserve(nnz); 1847 line_num = 2; 1848 } else { 1849 std::stringstream stream(line); 1850 casadi_int r, c; 1851 stream >> r; 1852 stream >> c; 1853 row.push_back(r-1); 1854 col.push_back(c-1); 1855 } 1856 } 1857 return triplet(size1, size2, row, col); 1858 } else { 1859 casadi_error("Unknown format '" + format + "'"); 1860 } 1861 } 1862 kkt(const Sparsity & H,const Sparsity & J,bool with_x_diag,bool with_lam_g_diag)1863 Sparsity Sparsity::kkt(const Sparsity& H, const Sparsity& J, 1864 bool with_x_diag, bool with_lam_g_diag) { 1865 // Consistency check 1866 casadi_assert(H.is_square(), "H must be square"); 1867 casadi_assert(H.size1() == J.size2(), "Dimension mismatch"); 1868 1869 // Add diagonal to H recursively 1870 if (with_x_diag) return kkt(H + diag(H.size()), J, false, with_lam_g_diag); 1871 1872 // Lower right entry 1873 int ng = J.size1(); 1874 Sparsity B = with_lam_g_diag ? diag(ng, ng) : Sparsity(ng, ng); 1875 1876 // Concatenate 1877 return blockcat({{H, J.T()}, {J, B}}); 1878 } 1879 serialize(std::ostream & stream) const1880 void Sparsity::serialize(std::ostream &stream) const { 1881 SerializingStream s(stream); 1882 serialize(s); 1883 } 1884 deserialize(std::istream & stream)1885 Sparsity Sparsity::deserialize(std::istream &stream) { 1886 DeserializingStream s(stream); 1887 return Sparsity::deserialize(s); 1888 } 1889 serialize(SerializingStream & s) const1890 void Sparsity::serialize(SerializingStream& s) const { 1891 if (is_null()) { 1892 s.pack("SparsityInternal::compressed", std::vector<casadi_int>{}); 1893 } else { 1894 s.pack("SparsityInternal::compressed", compress()); 1895 } 1896 } 1897 deserialize(DeserializingStream & s)1898 Sparsity Sparsity::deserialize(DeserializingStream& s) { 1899 std::vector<casadi_int> i; 1900 s.unpack("SparsityInternal::compressed", i); 1901 if (i.empty()) { 1902 return Sparsity(); 1903 } else { 1904 return Sparsity::compressed(i); 1905 } 1906 } 1907 serialize() const1908 std::string Sparsity::serialize() const { 1909 std::stringstream ss; 1910 serialize(ss); 1911 return ss.str(); 1912 } 1913 deserialize(const std::string & s)1914 Sparsity Sparsity::deserialize(const std::string& s) { 1915 std::stringstream ss; 1916 ss << s; 1917 return deserialize(ss); 1918 } 1919 get() const1920 SparsityInternal* Sparsity::get() const { 1921 return static_cast<SparsityInternal*>(SharedObject::get()); 1922 } 1923 } // namespace casadi 1924