1 // Copyright (C) 2004, 2006 International Business Machines and others. 2 // All Rights Reserved. 3 // This code is published under the Common Public License. 4 // 5 // $Id: IpCompoundMatrix.cpp 759 2006-07-07 03:07:08Z andreasw $ 6 // 7 // Authors: Carl Laird, Andreas Waechter IBM 2004-08-13 8 9 #include "IpoptConfig.h" 10 #include "IpCompoundMatrix.hpp" 11 #include "IpCompoundVector.hpp" 12 13 #ifdef HAVE_CSTDIO 14 # include <cstdio> 15 #else 16 # ifdef HAVE_STDIO_H 17 # include <stdio.h> 18 # else 19 # error "don't have header file for stdio" 20 # endif 21 #endif 22 23 // Keeps MS VC++ 8 quiet about sprintf, strcpy, etc. 24 #ifdef _MSC_VER 25 #pragma warning(disable:4996) 26 #endif 27 28 29 30 namespace SimTKIpopt 31 { 32 CompoundMatrix(const CompoundMatrixSpace * owner_space)33 CompoundMatrix::CompoundMatrix(const CompoundMatrixSpace* owner_space) 34 : 35 Matrix(owner_space), 36 owner_space_(owner_space), 37 matrices_valid_(false) 38 { 39 std::vector<SmartPtr<Matrix> > row(NComps_Cols()); 40 std::vector<SmartPtr<const Matrix> > const_row(NComps_Cols()); 41 for (Index irow=0; irow<NComps_Rows(); irow++) { 42 const_comps_.push_back(const_row); 43 comps_.push_back(row); 44 } 45 } 46 47 ~CompoundMatrix()48 CompoundMatrix::~CompoundMatrix() 49 {} 50 SetComp(Index irow,Index jcol,const Matrix & matrix)51 void CompoundMatrix::SetComp(Index irow, Index jcol, const Matrix& matrix) 52 { 53 DBG_ASSERT(irow<NComps_Rows()); 54 DBG_ASSERT(jcol<NComps_Cols()); 55 DBG_ASSERT(owner_space_->GetCompSpace(irow, jcol)->IsMatrixFromSpace(matrix)); 56 57 comps_[irow][jcol] = NULL; 58 const_comps_[irow][jcol] = &matrix; 59 ObjectChanged(); 60 } 61 SetCompNonConst(Index irow,Index jcol,Matrix & matrix)62 void CompoundMatrix::SetCompNonConst(Index irow, Index jcol, Matrix& matrix) 63 { 64 DBG_ASSERT(irow < NComps_Rows()); 65 DBG_ASSERT(jcol < NComps_Cols()); 66 DBG_ASSERT(owner_space_->GetCompSpace(irow, jcol)->IsMatrixFromSpace(matrix)); 67 68 const_comps_[irow][jcol] = NULL; 69 comps_[irow][jcol] = &matrix; 70 ObjectChanged(); 71 } 72 CreateBlockFromSpace(Index irow,Index jcol)73 void CompoundMatrix::CreateBlockFromSpace(Index irow, Index jcol) 74 { 75 DBG_ASSERT(irow < NComps_Rows()); 76 DBG_ASSERT(jcol < NComps_Cols()); 77 DBG_ASSERT(IsValid(owner_space_->GetCompSpace(irow, jcol))); 78 SetCompNonConst(irow, jcol, *owner_space_->GetCompSpace(irow,jcol)->MakeNew()); 79 } 80 MultVectorImpl(Number alpha,const Vector & x,Number beta,Vector & y) const81 void CompoundMatrix::MultVectorImpl(Number alpha, const Vector &x, 82 Number beta, Vector &y) const 83 { 84 if (!matrices_valid_) { 85 matrices_valid_ = MatricesValid(); 86 } 87 DBG_ASSERT(matrices_valid_); 88 89 // The vectors are assumed to be compound Vectors as well 90 const CompoundVector* comp_x = dynamic_cast<const CompoundVector*>(&x); 91 CompoundVector* comp_y = dynamic_cast<CompoundVector*>(&y); 92 93 // A few sanity checks 94 if (comp_x) { 95 DBG_ASSERT(NComps_Cols()==comp_x->NComps()); 96 } 97 else { 98 DBG_ASSERT(NComps_Cols() == 1); 99 } 100 101 if (comp_y) { 102 DBG_ASSERT(NComps_Rows()==comp_y->NComps()); 103 } 104 else { 105 DBG_ASSERT(NComps_Rows() == 1); 106 } 107 108 // Take care of the y part of the addition 109 if( beta!=0.0 ) { 110 y.Scal(beta); 111 } 112 else { 113 y.Set(0.0); // In case y hasn't been initialized yet 114 } 115 116 for( Index irow = 0; irow < NComps_Rows(); irow++ ) { 117 SmartPtr<Vector> y_i; 118 if (comp_y) { 119 y_i = comp_y->GetCompNonConst(irow); 120 } 121 else { 122 y_i = &y; 123 } 124 DBG_ASSERT(IsValid(y_i)); 125 126 for( Index jcol = 0; jcol < NComps_Cols(); jcol++ ) { 127 if ( (owner_space_->Diagonal() && irow == jcol) 128 || (!owner_space_->Diagonal() && ConstComp(irow,jcol)) ) { 129 SmartPtr<const Vector> x_j; 130 if (comp_x) { 131 x_j = comp_x->GetComp(jcol); 132 } 133 else if (NComps_Cols() == 1) { 134 x_j = &x; 135 } 136 DBG_ASSERT(IsValid(x_j)); 137 138 ConstComp(irow, jcol)->MultVector(alpha, *x_j, 139 1., *y_i); 140 } 141 } 142 } 143 } 144 TransMultVectorImpl(Number alpha,const Vector & x,Number beta,Vector & y) const145 void CompoundMatrix::TransMultVectorImpl(Number alpha, const Vector &x, 146 Number beta, Vector &y) const 147 { 148 if (!matrices_valid_) { 149 matrices_valid_ = MatricesValid(); 150 } 151 DBG_ASSERT(matrices_valid_); 152 153 // The vectors are assumed to be compound Vectors as well 154 const CompoundVector* comp_x = dynamic_cast<const CompoundVector*>(&x); 155 CompoundVector* comp_y = dynamic_cast<CompoundVector*>(&y); 156 157 // A few sanity checks 158 if (comp_y) { 159 DBG_ASSERT(NComps_Cols()==comp_y->NComps()); 160 } 161 else { 162 DBG_ASSERT(NComps_Cols() == 1); 163 } 164 165 if (comp_x) { 166 DBG_ASSERT(NComps_Rows()==comp_x->NComps()); 167 } 168 else { 169 DBG_ASSERT(NComps_Rows() == 1); 170 } 171 172 // Take care of the y part of the addition 173 if( beta!=0.0 ) { 174 y.Scal(beta); 175 } 176 else { 177 y.Set(0.0); // In case y hasn't been initialized yet 178 } 179 180 for( Index irow = 0; irow < NComps_Cols(); irow++ ) { 181 SmartPtr<Vector> y_i; 182 if (comp_y) { 183 y_i = comp_y->GetCompNonConst(irow); 184 } 185 else { 186 y_i = &y; 187 } 188 DBG_ASSERT(IsValid(y_i)); 189 190 for( Index jcol = 0; jcol < NComps_Rows(); jcol++ ) { 191 if ( (owner_space_->Diagonal() && irow == jcol) 192 || (!owner_space_->Diagonal() && ConstComp(jcol, irow)) ) { 193 SmartPtr<const Vector> x_j; 194 if (comp_x) { 195 x_j = comp_x->GetComp(jcol); 196 } 197 else { 198 x_j = &x; 199 } 200 DBG_ASSERT(IsValid(x_j)); 201 202 ConstComp(jcol, irow)->TransMultVector(alpha, *x_j, 203 1., *y_i); 204 } 205 } 206 } 207 } 208 209 // Specialized method (overloaded from IpMatrix) AddMSinvZImpl(Number alpha,const Vector & S,const Vector & Z,Vector & X) const210 void CompoundMatrix::AddMSinvZImpl(Number alpha, const Vector& S, 211 const Vector& Z, Vector& X) const 212 { 213 // The vectors are assumed to be compound Vectors as well (unless they 214 // are assumed to consist of only one component 215 const CompoundVector* comp_S = dynamic_cast<const CompoundVector*>(&S); 216 const CompoundVector* comp_Z = dynamic_cast<const CompoundVector*>(&Z); 217 CompoundVector* comp_X = dynamic_cast<CompoundVector*>(&X); 218 219 // A few sanity checks for sizes 220 if (comp_S) { 221 DBG_ASSERT(NComps_Cols()==comp_S->NComps()); 222 } 223 else { 224 DBG_ASSERT(NComps_Cols() == 1); 225 } 226 if (comp_Z) { 227 DBG_ASSERT(NComps_Cols()==comp_Z->NComps()); 228 } 229 else { 230 DBG_ASSERT(NComps_Cols() == 1); 231 } 232 if (comp_X) { 233 DBG_ASSERT(NComps_Rows()==comp_X->NComps()); 234 } 235 else { 236 DBG_ASSERT(NComps_Rows() == 1); 237 } 238 239 for( Index irow = 0; irow < NComps_Cols(); irow++ ) { 240 SmartPtr<Vector> X_i; 241 if (comp_X) { 242 X_i = comp_X->GetCompNonConst(irow); 243 } 244 else { 245 X_i = &X; 246 } 247 DBG_ASSERT(IsValid(X_i)); 248 249 for( Index jcol = 0; jcol < NComps_Cols(); jcol++ ) { 250 if ( (owner_space_->Diagonal() && irow == jcol) 251 || (!owner_space_->Diagonal() && ConstComp(jcol, irow)) ) { 252 SmartPtr<const Vector> S_j; 253 if (comp_S) { 254 S_j = comp_S->GetComp(jcol); 255 } 256 else { 257 S_j = &S; 258 } 259 DBG_ASSERT(IsValid(S_j)); 260 SmartPtr<const Vector> Z_j; 261 if (comp_Z) { 262 Z_j = comp_Z->GetComp(jcol); 263 } 264 else { 265 Z_j = &Z; 266 } 267 DBG_ASSERT(IsValid(Z_j)); 268 269 ConstComp(jcol, irow)->AddMSinvZ(alpha, *S_j, *Z_j, *X_i); 270 } 271 } 272 } 273 } 274 275 // Specialized method (overloaded from IpMatrix) SinvBlrmZMTdBrImpl(Number alpha,const Vector & S,const Vector & R,const Vector & Z,const Vector & D,Vector & X) const276 void CompoundMatrix::SinvBlrmZMTdBrImpl(Number alpha, const Vector& S, 277 const Vector& R, const Vector& Z, 278 const Vector& D, Vector& X) const 279 { 280 // First check if the matrix is indeed such that we can use the 281 // special methods from the component spaces (this only works if 282 // we have exactly one submatrix per column) 283 // CDL: in every case this was true, the matrix blocks were on the 284 // diagonal so I made this more efficient. 285 286 if (!owner_space_->Diagonal()) { 287 // Use the standard replacement implementation 288 Matrix::SinvBlrmZMTdBrImpl(alpha, S, R, Z, D, X); 289 DBG_ASSERT(false && "Found a matrix where we can't use the fast SinvBlrmZMTdBr implementation in CompoundMatrix"); 290 } 291 else { 292 // The vectors are assumed to be compound Vectors as well (unless they 293 // are assumed to consist of only one component 294 const CompoundVector* comp_S = dynamic_cast<const CompoundVector*>(&S); 295 const CompoundVector* comp_R = dynamic_cast<const CompoundVector*>(&R); 296 const CompoundVector* comp_Z = dynamic_cast<const CompoundVector*>(&Z); 297 const CompoundVector* comp_D = dynamic_cast<const CompoundVector*>(&D); 298 CompoundVector* comp_X = dynamic_cast<CompoundVector*>(&X); 299 300 // A few sanity checks for sizes 301 if (comp_S) { 302 DBG_ASSERT(NComps_Cols()==comp_S->NComps()); 303 } 304 else { 305 DBG_ASSERT(NComps_Cols() == 1); 306 } 307 if (comp_Z) { 308 DBG_ASSERT(NComps_Cols()==comp_Z->NComps()); 309 } 310 else { 311 DBG_ASSERT(NComps_Cols() == 1); 312 } 313 if (comp_R) { 314 DBG_ASSERT(NComps_Cols()==comp_R->NComps()); 315 } 316 else { 317 DBG_ASSERT(NComps_Cols() == 1); 318 } 319 if (comp_D) { 320 DBG_ASSERT(NComps_Rows()==comp_D->NComps()); 321 } 322 else { 323 DBG_ASSERT(NComps_Rows() == 1); 324 } 325 if (comp_X) { 326 DBG_ASSERT(NComps_Cols()==comp_X->NComps()); 327 } 328 else { 329 DBG_ASSERT(NComps_Cols() == 1); 330 } 331 332 for (Index irow=0; irow<NComps_Cols(); irow++ ) { 333 Index jcol = irow; // diagonal, remember 334 SmartPtr<const Vector> S_i; 335 if (comp_S) { 336 S_i = comp_S->GetComp(irow); 337 } 338 else { 339 S_i = &S; 340 } 341 DBG_ASSERT(IsValid(S_i)); 342 SmartPtr<const Vector> Z_i; 343 if (comp_Z) { 344 Z_i = comp_Z->GetComp(irow); 345 } 346 else { 347 Z_i = &Z; 348 } 349 DBG_ASSERT(IsValid(Z_i)); 350 SmartPtr<const Vector> R_i; 351 if (comp_R) { 352 R_i = comp_R->GetComp(irow); 353 } 354 else { 355 R_i = &R; 356 } 357 DBG_ASSERT(IsValid(R_i)); 358 SmartPtr<const Vector> D_i; 359 if (comp_D) { 360 D_i = comp_D->GetComp(jcol); 361 } 362 else { 363 D_i = &D; 364 } 365 DBG_ASSERT(IsValid(D_i)); 366 SmartPtr<Vector> X_i; 367 if (comp_X) { 368 X_i = comp_X->GetCompNonConst(irow); 369 } 370 else { 371 X_i = &X; 372 } 373 DBG_ASSERT(IsValid(X_i)); 374 375 ConstComp(jcol, irow)->SinvBlrmZMTdBr(alpha, *S_i, *R_i, *Z_i, 376 *D_i, *X_i); 377 } 378 } 379 } 380 HasValidNumbersImpl() const381 bool CompoundMatrix::HasValidNumbersImpl() const 382 { 383 if (!matrices_valid_) { 384 matrices_valid_ = MatricesValid(); 385 } 386 DBG_ASSERT(matrices_valid_); 387 388 for( Index irow = 0; irow < NComps_Rows(); irow++ ) { 389 for( Index jcol = 0; jcol < NComps_Cols(); jcol++ ) { 390 if ( (owner_space_->Diagonal() && irow == jcol) 391 || (!owner_space_->Diagonal() && ConstComp(irow,jcol)) ) { 392 if (!ConstComp(irow, jcol)->HasValidNumbers()) { 393 return false; 394 } 395 } 396 } 397 } 398 return true; 399 } 400 PrintImpl(const Journalist & jnlst,EJournalLevel level,EJournalCategory category,const std::string & name,Index indent,const std::string & prefix) const401 void CompoundMatrix::PrintImpl(const Journalist& jnlst, 402 EJournalLevel level, 403 EJournalCategory category, 404 const std::string& name, 405 Index indent, 406 const std::string& prefix) const 407 { 408 jnlst.Printf(level, category, "\n"); 409 jnlst.PrintfIndented(level, category, indent, 410 "%sCompoundMatrix \"%s\" with %d row and %d columns components:\n", 411 prefix.c_str(), name.c_str(), 412 NComps_Rows(), NComps_Cols()); 413 for (Index irow = 0; irow < NComps_Rows(); irow++ ) { 414 for (Index jcol = 0; jcol < NComps_Cols(); jcol++ ) { 415 jnlst.PrintfIndented(level, category, indent, 416 "%sComponent for row %d and column %d:\n", 417 prefix.c_str(), irow, jcol); 418 if (ConstComp(irow, jcol)) { 419 DBG_ASSERT(name.size()<200); 420 char buffer[256]; 421 sprintf(buffer, "%s[%2d][%2d]", name.c_str(), irow, jcol); 422 std::string term_name = buffer; 423 ConstComp(irow, jcol)->Print(&jnlst, level, category, term_name, 424 indent+1, prefix); 425 } 426 else { 427 jnlst.PrintfIndented(level, category, indent, 428 "%sComponent has not been set.\n", 429 prefix.c_str()); 430 } 431 } 432 } 433 } 434 MatricesValid() const435 bool CompoundMatrix::MatricesValid() const 436 { 437 // Check to make sure we have matrices everywhere the space has matrices 438 // We already check that the matrix agrees with the block space 439 // in the SetComp methods 440 bool retValue = true; 441 for (Index i=0; i<NComps_Rows(); i++) { 442 for (Index j=0; j<NComps_Cols(); j++) { 443 if ( (!ConstComp(i,j) && IsValid(owner_space_->GetCompSpace(i,j))) 444 || (ConstComp(i,j) && IsNull(owner_space_->GetCompSpace(i,j))) ) { 445 retValue = false; 446 break; 447 } 448 } 449 } 450 return retValue; 451 } 452 453 CompoundMatrixSpace(Index ncomps_rows,Index ncomps_cols,Index total_nRows,Index total_nCols)454 CompoundMatrixSpace::CompoundMatrixSpace(Index ncomps_rows, Index ncomps_cols, Index total_nRows, Index total_nCols) 455 : 456 MatrixSpace(total_nRows, total_nCols), 457 ncomps_rows_(ncomps_rows), 458 ncomps_cols_(ncomps_cols), 459 dimensions_set_(false), 460 block_rows_(ncomps_rows, -1), 461 block_cols_(ncomps_cols, -1), 462 diagonal_(false) 463 { 464 DBG_START_METH("CompoundMatrixSpace::CompoundMatrixSpace", 0); 465 std::vector<SmartPtr<const MatrixSpace> > row(ncomps_cols_); 466 std::vector< bool > allocate_row(ncomps_cols_, false); 467 for (Index i=0; i<ncomps_rows_; i++) { 468 DBG_PRINT((1, "block_rows_[%d] = %d\n", i, block_rows_[i])); 469 comp_spaces_.push_back(row); 470 allocate_block_.push_back(allocate_row); 471 } 472 } 473 SetBlockCols(Index jcol,Index ncols)474 void CompoundMatrixSpace::SetBlockCols(Index jcol, Index ncols) 475 { 476 DBG_ASSERT(!dimensions_set_ && "for now, if the dimensions have all been set, they cannot be changed"); 477 DBG_ASSERT(jcol < ncomps_cols_); 478 DBG_ASSERT(block_cols_[jcol] == -1 && "This dimension has already been set - sanity check"); 479 block_cols_[jcol] = ncols; 480 } 481 SetBlockRows(Index irow,Index nrows)482 void CompoundMatrixSpace::SetBlockRows(Index irow, Index nrows) 483 { 484 DBG_ASSERT(!dimensions_set_ && "for now, if the dimensions have all been set, they cannot be changed"); 485 DBG_ASSERT(irow < ncomps_rows_); 486 DBG_ASSERT(block_rows_[irow] == -1 && "This dimension has already been set - sanity check"); 487 block_rows_[irow] = nrows; 488 } 489 GetBlockRows(Index irow) const490 Index CompoundMatrixSpace::GetBlockRows(Index irow) const 491 { 492 DBG_ASSERT(dimensions_set_); 493 DBG_ASSERT(irow < ncomps_rows_); 494 return block_rows_[irow]; 495 } 496 GetBlockCols(Index jcol) const497 Index CompoundMatrixSpace::GetBlockCols(Index jcol) const 498 { 499 DBG_ASSERT(dimensions_set_); 500 DBG_ASSERT(jcol < ncomps_cols_); 501 return block_cols_[jcol]; 502 } 503 SetCompSpace(Index irow,Index jcol,const MatrixSpace & mat_space,bool auto_allocate)504 void CompoundMatrixSpace::SetCompSpace(Index irow, Index jcol, 505 const MatrixSpace& mat_space, 506 bool auto_allocate /*=false*/) 507 { 508 if (!dimensions_set_) { 509 dimensions_set_ = DimensionsSet(); 510 } 511 DBG_ASSERT(dimensions_set_); 512 DBG_ASSERT(irow<ncomps_rows_); 513 DBG_ASSERT(jcol<ncomps_cols_); 514 DBG_ASSERT(IsNull(comp_spaces_[irow][jcol])); 515 DBG_ASSERT(block_cols_[jcol] != -1 && block_cols_[jcol] == mat_space.NCols()); 516 DBG_ASSERT(block_rows_[irow] != -1 && block_rows_[irow] == mat_space.NRows()); 517 518 comp_spaces_[irow][jcol] = &mat_space; 519 allocate_block_[irow][jcol] = auto_allocate; 520 521 diagonal_ = true; 522 for (Index i=0; i < NComps_Rows(); i++) { 523 for (Index j=0; j < NComps_Cols(); j++) { 524 if ( (i == j && IsNull(GetCompSpace(i,j))) 525 || (i != j && IsValid(GetCompSpace(i,j)))) { 526 diagonal_ = false; 527 break; 528 } 529 } 530 } 531 } 532 MakeNewCompoundMatrix() const533 CompoundMatrix* CompoundMatrixSpace::MakeNewCompoundMatrix() const 534 { 535 if (!dimensions_set_) { 536 dimensions_set_ = DimensionsSet(); 537 } 538 DBG_ASSERT(dimensions_set_); 539 540 CompoundMatrix* mat = new CompoundMatrix(this); 541 for(Index i=0; i<ncomps_rows_; i++) { 542 for (Index j=0; j<ncomps_cols_; j++) { 543 if (allocate_block_[i][j]) { 544 mat->SetCompNonConst(i, j, *GetCompSpace(i, j)->MakeNew()); 545 } 546 } 547 } 548 549 return mat; 550 } 551 DimensionsSet() const552 bool CompoundMatrixSpace::DimensionsSet() const 553 { 554 DBG_START_METH("CompoundMatrixSpace::DimensionsSet", 0); 555 Index total_nrows = 0; 556 Index total_ncols = 0; 557 bool valid = true; 558 for (Index i=0; i<ncomps_rows_; i++) { 559 if (block_rows_[i] == -1) { 560 valid = false; 561 break; 562 } 563 total_nrows += block_rows_[i]; 564 } 565 if (valid) { 566 for (Index j=0; j<ncomps_cols_; j++) { 567 if (block_cols_[j] == -1) { 568 valid = false; 569 break; 570 } 571 total_ncols += block_cols_[j]; 572 } 573 } 574 575 if (valid) { 576 DBG_ASSERT(total_nrows == NRows() && total_ncols == NCols()); 577 } 578 579 return valid; 580 } 581 582 } // namespace Ipopt 583