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: IpDenseVector.cpp 759 2006-07-07 03:07:08Z andreasw $ 6 // 7 // Authors: Carl Laird, Andreas Waechter IBM 2004-08-13 8 9 #include "IpDenseVector.hpp" 10 #include "IpBlas.hpp" 11 #include "IpUtils.hpp" 12 #include "IpDebug.hpp" 13 14 #ifdef HAVE_CMATH 15 # include <cmath> 16 #else 17 # ifdef HAVE_MATH_H 18 # include <math.h> 19 # else 20 # error "don't have header file for math" 21 # endif 22 #endif 23 24 namespace SimTKIpopt 25 { 26 27 #ifdef IP_DEBUG 28 static const Index dbg_verbosity = 0; 29 #endif 30 DenseVector(const DenseVectorSpace * owner_space)31 DenseVector::DenseVector(const DenseVectorSpace* owner_space) 32 : 33 Vector(owner_space), 34 owner_space_(owner_space), 35 values_(NULL), 36 initialized_(false) 37 { 38 DBG_START_METH("DenseVector::DenseVector(Index dim)", dbg_verbosity); 39 if (Dim() == 0) { 40 initialized_ = true; 41 homogeneous_ = false; 42 } 43 } 44 ~DenseVector()45 DenseVector::~DenseVector() 46 { 47 DBG_START_METH("DenseVector::~DenseVector()", dbg_verbosity); 48 if (values_) { 49 owner_space_->FreeInternalStorage(values_); 50 } 51 } 52 SetValues(const Number * x)53 void DenseVector::SetValues(const Number* x) 54 { 55 initialized_ = true; 56 IpBlasDcopy(Dim(), x, 1, values_allocated(), 1); 57 homogeneous_ = false; 58 // This is not an overloaded method from 59 // Vector. Here, we must call ObjectChanged() 60 // manually. 61 ObjectChanged(); 62 } 63 set_values_from_scalar()64 void DenseVector::set_values_from_scalar() 65 { 66 DBG_ASSERT(homogeneous_); 67 initialized_ = true; 68 homogeneous_ = false; 69 Number* vals = values_allocated(); 70 IpBlasDcopy(Dim(), &scalar_, 0, vals, 1); 71 } 72 CopyImpl(const Vector & x)73 void DenseVector::CopyImpl(const Vector& x) 74 { 75 DBG_START_METH("DenseVector::CopyImpl(const Vector& x)", dbg_verbosity); 76 const DenseVector* dense_x = dynamic_cast<const DenseVector*>(&x); 77 DBG_ASSERT(dense_x); 78 if (dense_x) { 79 DBG_ASSERT(dense_x->initialized_); 80 DBG_ASSERT(Dim() == dense_x->Dim()); 81 homogeneous_ = dense_x->homogeneous_; 82 if (homogeneous_) { 83 scalar_ = dense_x->scalar_; 84 } 85 else { 86 IpBlasDcopy(Dim(), dense_x->values_, 1, values_allocated(), 1); 87 } 88 } 89 initialized_=true; 90 } 91 ScalImpl(Number alpha)92 void DenseVector::ScalImpl(Number alpha) 93 { 94 DBG_ASSERT(initialized_); 95 if (homogeneous_) { 96 scalar_ *= alpha; 97 } 98 else { 99 IpBlasDscal(Dim(), alpha, values_, 1); 100 } 101 } 102 AxpyImpl(Number alpha,const Vector & x)103 void DenseVector::AxpyImpl(Number alpha, const Vector &x) 104 { 105 DBG_ASSERT(initialized_); 106 const DenseVector* dense_x = dynamic_cast<const DenseVector*>(&x); 107 DBG_ASSERT(dense_x); 108 if (dense_x) { 109 DBG_ASSERT(dense_x->initialized_); 110 DBG_ASSERT(Dim() == dense_x->Dim()); 111 if (homogeneous_) { 112 if (dense_x->homogeneous_) { 113 scalar_ += alpha * dense_x->scalar_; 114 } 115 else { 116 homogeneous_ = false; 117 Number* vals = values_allocated(); 118 for (Index i=0; i<Dim(); i++) { 119 vals[i] = scalar_ + alpha*dense_x->values_[i]; 120 } 121 } 122 } 123 else { 124 if (dense_x->homogeneous_) { 125 if (dense_x->scalar_!=0.) { 126 IpBlasDaxpy(Dim(), alpha, &dense_x->scalar_, 0, values_, 1); 127 } 128 } 129 else { 130 IpBlasDaxpy(Dim(), alpha, dense_x->values_, 1, values_, 1); 131 } 132 } 133 } 134 } 135 DotImpl(const Vector & x) const136 Number DenseVector::DotImpl(const Vector &x) const 137 { 138 DBG_ASSERT(initialized_); 139 Number retValue; 140 const DenseVector* dense_x = dynamic_cast<const DenseVector*>(&x); 141 DBG_ASSERT(dense_x); 142 DBG_ASSERT(dense_x->initialized_); 143 DBG_ASSERT(Dim() == dense_x->Dim()); 144 if (homogeneous_) { 145 if (dense_x->homogeneous_) { 146 retValue = Dim() * scalar_ * dense_x->scalar_; 147 } 148 else { 149 retValue = IpBlasDdot(Dim(), dense_x->values_, 1, &scalar_, 0); 150 } 151 } 152 else { 153 if (dense_x->homogeneous_) { 154 retValue = IpBlasDdot(Dim(), &dense_x->scalar_, 0, values_, 1); 155 } 156 else { 157 retValue = IpBlasDdot(Dim(), dense_x->values_, 1, values_, 1); 158 } 159 } 160 return retValue; 161 } 162 Nrm2Impl() const163 Number DenseVector::Nrm2Impl() const 164 { 165 DBG_ASSERT(initialized_); 166 if (homogeneous_) { 167 return sqrt((double)Dim()) * fabs(scalar_); 168 } 169 else { 170 return IpBlasDnrm2(Dim(), values_, 1); 171 } 172 } 173 AsumImpl() const174 Number DenseVector::AsumImpl() const 175 { 176 DBG_ASSERT(initialized_); 177 if (homogeneous_) { 178 return Dim() * fabs(scalar_); 179 } 180 else { 181 return IpBlasDasum(Dim(), values_, 1); 182 } 183 } 184 AmaxImpl() const185 Number DenseVector::AmaxImpl() const 186 { 187 DBG_ASSERT(initialized_); 188 if (Dim()==0) { 189 return 0.; 190 } 191 else { 192 if (homogeneous_) { 193 return fabs(scalar_); 194 } 195 else { 196 return fabs(values_[IpBlasIdamax(Dim(), values_, 1)-1]); 197 } 198 } 199 } 200 SetImpl(Number value)201 void DenseVector::SetImpl(Number value) 202 { 203 initialized_ = true; 204 homogeneous_ = true; 205 scalar_ = value; 206 // ToDo decide if we want this here: 207 if (values_) { 208 owner_space_->FreeInternalStorage(values_); 209 values_ = NULL; 210 } 211 } 212 ElementWiseDivideImpl(const Vector & x)213 void DenseVector::ElementWiseDivideImpl(const Vector& x) 214 { 215 DBG_ASSERT(initialized_); 216 const DenseVector* dense_x = dynamic_cast<const DenseVector*>(&x); 217 DBG_ASSERT(dense_x); 218 if (dense_x) { 219 DBG_ASSERT(dense_x->initialized_); 220 const Number* values_x = dense_x->values_; 221 DBG_ASSERT(Dim() == dense_x->Dim()); 222 if (homogeneous_) { 223 if (dense_x->homogeneous_) { 224 scalar_ /= dense_x->scalar_; 225 } 226 else { 227 homogeneous_ = false; 228 Number* vals = values_allocated(); 229 for (Index i=0; i<Dim(); i++) { 230 vals[i] = scalar_/values_x[i]; 231 } 232 } 233 } 234 else { 235 if (dense_x->homogeneous_) { 236 for (Index i=0; i<Dim(); i++) { 237 values_[i] /= dense_x->scalar_; 238 } 239 } 240 else { 241 for (Index i=0; i<Dim(); i++) { 242 values_[i] /= values_x[i]; 243 } 244 } 245 } 246 } 247 } 248 ElementWiseMultiplyImpl(const Vector & x)249 void DenseVector::ElementWiseMultiplyImpl(const Vector& x) 250 { 251 DBG_ASSERT(initialized_); 252 const DenseVector* dense_x = dynamic_cast<const DenseVector*>(&x); 253 DBG_ASSERT(dense_x); 254 if (dense_x) { 255 DBG_ASSERT(dense_x->initialized_); 256 const Number* values_x = dense_x->values_; 257 DBG_ASSERT(Dim() == dense_x->Dim()); 258 if (homogeneous_) { 259 if (dense_x->homogeneous_) { 260 scalar_ *= dense_x->scalar_; 261 } 262 else { 263 homogeneous_ = false; 264 Number* vals = values_allocated(); 265 for (Index i=0; i<Dim(); i++) { 266 vals[i] = scalar_*values_x[i]; 267 } 268 } 269 } 270 else { 271 if (dense_x->homogeneous_) { 272 if (dense_x->scalar_ != 1.0) { 273 for (Index i=0; i<Dim(); i++) { 274 values_[i] *= dense_x->scalar_; 275 } 276 } 277 } 278 else { 279 for (Index i=0; i<Dim(); i++) { 280 values_[i] *= values_x[i]; 281 } 282 } 283 } 284 } 285 } 286 ElementWiseMaxImpl(const Vector & x)287 void DenseVector::ElementWiseMaxImpl(const Vector& x) 288 { 289 DBG_ASSERT(initialized_); 290 const DenseVector* dense_x = dynamic_cast<const DenseVector*>(&x); 291 assert(dense_x); // ToDo: Implement Others 292 if (dense_x) { 293 DBG_ASSERT(dense_x->initialized_); 294 const Number* values_x = dense_x->values_; 295 DBG_ASSERT(Dim() == dense_x->Dim()); 296 if (homogeneous_) { 297 if (dense_x->homogeneous_) { 298 scalar_ = SimTKIpopt::Max(scalar_, dense_x->scalar_); 299 } 300 else { 301 homogeneous_ = false; 302 Number* vals = values_allocated(); 303 for (Index i=0; i<Dim(); i++) { 304 vals[i] = SimTKIpopt::Max(scalar_, values_x[i]); 305 } 306 } 307 } 308 else { 309 if (dense_x->homogeneous_) { 310 for (Index i=0; i<Dim(); i++) { 311 values_[i] = SimTKIpopt::Max(values_[i], dense_x->scalar_); 312 } 313 } 314 else { 315 for (Index i=0; i<Dim(); i++) { 316 values_[i] = SimTKIpopt::Max(values_[i], values_x[i]); 317 } 318 } 319 } 320 } 321 } 322 ElementWiseMinImpl(const Vector & x)323 void DenseVector::ElementWiseMinImpl(const Vector& x) 324 { 325 DBG_ASSERT(initialized_); 326 const DenseVector* dense_x = dynamic_cast<const DenseVector*>(&x); 327 DBG_ASSERT(dense_x); 328 if (dense_x) { 329 DBG_ASSERT(dense_x->initialized_); 330 const Number* values_x = dense_x->values_; 331 DBG_ASSERT(Dim() == dense_x->Dim()); 332 if (homogeneous_) { 333 if (dense_x->homogeneous_) { 334 scalar_ = SimTKIpopt::Min(scalar_, dense_x->scalar_); 335 } 336 else { 337 homogeneous_ = false; 338 Number* vals = values_allocated(); 339 for (Index i=0; i<Dim(); i++) { 340 vals[i] = SimTKIpopt::Min(scalar_, values_x[i]); 341 } 342 } 343 } 344 else { 345 if (dense_x->homogeneous_) { 346 for (Index i=0; i<Dim(); i++) { 347 values_[i] = SimTKIpopt::Min(values_[i], dense_x->scalar_); 348 } 349 } 350 else { 351 for (Index i=0; i<Dim(); i++) { 352 values_[i] = SimTKIpopt::Min(values_[i], values_x[i]); 353 } 354 } 355 } 356 } 357 } 358 ElementWiseReciprocalImpl()359 void DenseVector::ElementWiseReciprocalImpl() 360 { 361 DBG_ASSERT(initialized_); 362 if (homogeneous_) { 363 scalar_ = 1.0/scalar_; 364 } 365 else { 366 for (Index i=0; i<Dim(); i++) { 367 values_[i] = 1.0/values_[i]; 368 } 369 } 370 } 371 ElementWiseAbsImpl()372 void DenseVector::ElementWiseAbsImpl() 373 { 374 DBG_ASSERT(initialized_); 375 if (homogeneous_) { 376 scalar_ = fabs(scalar_); 377 } 378 else { 379 for (Index i=0; i<Dim(); i++) { 380 values_[i] = fabs(values_[i]); 381 } 382 } 383 } 384 ElementWiseSqrtImpl()385 void DenseVector::ElementWiseSqrtImpl() 386 { 387 DBG_ASSERT(initialized_); 388 if (homogeneous_) { 389 scalar_ = sqrt(scalar_); 390 } 391 else { 392 for (Index i=0; i<Dim(); i++) { 393 values_[i] = sqrt(values_[i]); 394 } 395 } 396 } 397 AddScalarImpl(Number scalar)398 void DenseVector::AddScalarImpl(Number scalar) 399 { 400 DBG_ASSERT(initialized_); 401 if (homogeneous_) { 402 scalar_ += scalar; 403 } 404 else { 405 IpBlasDaxpy(Dim(), 1., &scalar, 0, values_, 1); 406 } 407 } 408 MaxImpl() const409 Number DenseVector::MaxImpl() const 410 { 411 DBG_ASSERT(initialized_); 412 DBG_ASSERT(Dim() > 0 && "There is no Max of a zero length vector (no reasonable default can be returned)"); 413 Number max; 414 if (homogeneous_) { 415 max = scalar_; 416 } 417 else { 418 max = values_[0]; 419 for (Index i=1; i<Dim(); i++) { 420 max = SimTKIpopt::Max(values_[i], max); 421 } 422 } 423 return max; 424 } 425 MinImpl() const426 Number DenseVector::MinImpl() const 427 { 428 DBG_ASSERT(initialized_); 429 DBG_ASSERT(Dim() > 0 && "There is no Min of a zero length vector" 430 "(no reasonable default can be returned) - " 431 "Check for zero length vector before calling"); 432 Number min; 433 if (homogeneous_) { 434 min = scalar_; 435 } 436 else { 437 min = values_[0]; 438 for (Index i=1; i<Dim(); i++) { 439 min = SimTKIpopt::Min(values_[i], min); 440 } 441 } 442 return min; 443 } 444 SumImpl() const445 Number DenseVector::SumImpl() const 446 { 447 DBG_ASSERT(initialized_); 448 Number sum; 449 if (homogeneous_) { 450 sum = Dim()*scalar_; 451 } 452 else { 453 sum = 0.; 454 for (Index i=0; i<Dim(); i++) { 455 sum += values_[i]; 456 } 457 } 458 return sum; 459 } 460 SumLogsImpl() const461 Number DenseVector::SumLogsImpl() const 462 { 463 DBG_ASSERT(initialized_); 464 Number sum; 465 if (homogeneous_) { 466 sum = Dim() * log(scalar_); 467 } 468 else { 469 sum = 0.0; 470 for (Index i=0; i<Dim(); i++) { 471 sum += log(values_[i]); 472 } 473 } 474 return sum; 475 } 476 ElementWiseSgnImpl()477 void DenseVector::ElementWiseSgnImpl() 478 { 479 DBG_ASSERT(initialized_); 480 if (homogeneous_) { 481 if (scalar_ > 0.) { 482 scalar_ = 1.; 483 } 484 else if (scalar_ < 0.) { 485 scalar_ = -1.; 486 } 487 else { 488 scalar_ = 0.; 489 } 490 } 491 else { 492 for (Index i=0; i<Dim(); i++) { 493 if (values_[i] > 0.) { 494 values_[i] = 1.; 495 } 496 else if (values_[i] < 0.) { 497 values_[i] = -1.; 498 } 499 else { 500 values_[i] = 0; 501 } 502 } 503 } 504 } 505 506 // Specialized Functions AddTwoVectorsImpl(Number a,const Vector & v1,Number b,const Vector & v2,Number c)507 void DenseVector::AddTwoVectorsImpl(Number a, const Vector& v1, 508 Number b, const Vector& v2, Number c) 509 { 510 Number* values_v1=NULL; 511 bool homogeneous_v1=false; 512 Number scalar_v1 = 0; 513 if (a!=0.) { 514 const DenseVector* dense_v1 = dynamic_cast<const DenseVector*>(&v1); 515 DBG_ASSERT(dense_v1); 516 DBG_ASSERT(dense_v1->initialized_); 517 DBG_ASSERT(Dim() == dense_v1->Dim()); 518 values_v1=dense_v1->values_; 519 homogeneous_v1=dense_v1->homogeneous_; 520 if (homogeneous_v1) 521 scalar_v1 = dense_v1->scalar_; 522 } 523 Number* values_v2=NULL; 524 bool homogeneous_v2=false; 525 Number scalar_v2 = 0; 526 if (b!=0.) { 527 const DenseVector* dense_v2 = dynamic_cast<const DenseVector*>(&v2); 528 DBG_ASSERT(dense_v2); 529 DBG_ASSERT(dense_v2->initialized_); 530 DBG_ASSERT(Dim() == dense_v2->Dim()); 531 values_v2=dense_v2->values_; 532 homogeneous_v2=dense_v2->homogeneous_; 533 if (homogeneous_v2) 534 scalar_v2 = dense_v2->scalar_; 535 } 536 DBG_ASSERT(c==0. || initialized_); 537 if ((c==0. || homogeneous_) && homogeneous_v1 && homogeneous_v2 ) { 538 homogeneous_ = true; 539 Number val = 0; 540 if (c!=0.) { 541 val = c*scalar_; 542 } 543 scalar_ = val + a*scalar_v1 + b*scalar_v2; 544 initialized_ = true; 545 return; 546 } 547 if (c==0.) { 548 // make sure we have memory allocated for this vector 549 values_allocated(); 550 homogeneous_ = false; 551 } 552 553 // If any of the vectors is homogeneous, call the default implementation 554 if ( homogeneous_ || homogeneous_v1 || homogeneous_v2) { 555 // ToDo:Should we implement specialized methods here too? 556 Vector::AddTwoVectorsImpl(a, v1, b, v2, c); 557 return; 558 } 559 560 // I guess I'm going over board here, but it might be best to 561 // capture all cases for a, b, and c separately... 562 if (c==0 ) { 563 if (a==1.) { 564 if (b==0.) { 565 for (Index i=0; i<Dim(); i++) { 566 values_[i] = values_v1[i]; 567 } 568 } 569 else if (b==1.) { 570 for (Index i=0; i<Dim(); i++) { 571 values_[i] = values_v1[i] + values_v2[i]; 572 } 573 } 574 else if (b==-1.) { 575 for (Index i=0; i<Dim(); i++) { 576 values_[i] = values_v1[i] - values_v2[i]; 577 } 578 } 579 else { 580 for (Index i=0; i<Dim(); i++) { 581 values_[i] = values_v1[i] + b*values_v2[i]; 582 } 583 } 584 } 585 else if (a==-1.) { 586 if (b==0.) { 587 for (Index i=0; i<Dim(); i++) { 588 values_[i] = -values_v1[i]; 589 } 590 } 591 else if (b==1.) { 592 for (Index i=0; i<Dim(); i++) { 593 values_[i] = -values_v1[i] + values_v2[i]; 594 } 595 } 596 else if (b==-1.) { 597 for (Index i=0; i<Dim(); i++) { 598 values_[i] = -values_v1[i] - values_v2[i]; 599 } 600 } 601 else { 602 for (Index i=0; i<Dim(); i++) { 603 values_[i] = -values_v1[i] + b*values_v2[i]; 604 } 605 } 606 } 607 else if (a==0.) { 608 if (b==0.) { 609 for (Index i=0; i<Dim(); i++) { 610 values_[i] = 0.; 611 } 612 } 613 else if (b==1.) { 614 for (Index i=0; i<Dim(); i++) { 615 values_[i] = values_v2[i]; 616 } 617 } 618 else if (b==-1.) { 619 for (Index i=0; i<Dim(); i++) { 620 values_[i] = -values_v2[i]; 621 } 622 } 623 else { 624 for (Index i=0; i<Dim(); i++) { 625 values_[i] = b*values_v2[i]; 626 } 627 } 628 } 629 else { 630 if (b==0.) { 631 for (Index i=0; i<Dim(); i++) { 632 values_[i] = a*values_v1[i]; 633 } 634 } 635 else if (b==1.) { 636 for (Index i=0; i<Dim(); i++) { 637 values_[i] = a*values_v1[i] + values_v2[i]; 638 } 639 } 640 else if (b==-1.) { 641 for (Index i=0; i<Dim(); i++) { 642 values_[i] = a*values_v1[i] - values_v2[i]; 643 } 644 } 645 else { 646 for (Index i=0; i<Dim(); i++) { 647 values_[i] = a*values_v1[i] + b*values_v2[i]; 648 } 649 } 650 } 651 } 652 else if (c==1.) { 653 if (a==1.) { 654 if (b==0.) { 655 for (Index i=0; i<Dim(); i++) { 656 values_[i] += values_v1[i]; 657 } 658 } 659 else if (b==1.) { 660 for (Index i=0; i<Dim(); i++) { 661 values_[i] += values_v1[i] + values_v2[i]; 662 } 663 } 664 else if (b==-1.) { 665 for (Index i=0; i<Dim(); i++) { 666 values_[i] += values_v1[i] - values_v2[i]; 667 } 668 } 669 else { 670 for (Index i=0; i<Dim(); i++) { 671 values_[i] += values_v1[i] + b*values_v2[i]; 672 } 673 } 674 } 675 else if (a==-1.) { 676 if (b==0.) { 677 for (Index i=0; i<Dim(); i++) { 678 values_[i] -= values_v1[i]; 679 } 680 } 681 else if (b==1.) { 682 for (Index i=0; i<Dim(); i++) { 683 values_[i] += -values_v1[i] + values_v2[i]; 684 } 685 } 686 else if (b==-1.) { 687 for (Index i=0; i<Dim(); i++) { 688 values_[i] += -values_v1[i] - values_v2[i]; 689 } 690 } 691 else { 692 for (Index i=0; i<Dim(); i++) { 693 values_[i] += -values_v1[i] + b*values_v2[i]; 694 } 695 } 696 } 697 else if (a==0.) { 698 if (b==0.) { 699 /* Nothing */ 700 } 701 else if (b==1.) { 702 for (Index i=0; i<Dim(); i++) { 703 values_[i] += values_v2[i]; 704 } 705 } 706 else if (b==-1.) { 707 for (Index i=0; i<Dim(); i++) { 708 values_[i] -= values_v2[i]; 709 } 710 } 711 else { 712 for (Index i=0; i<Dim(); i++) { 713 values_[i] += b*values_v2[i]; 714 } 715 } 716 } 717 else { 718 if (b==0.) { 719 for (Index i=0; i<Dim(); i++) { 720 values_[i] += a*values_v1[i]; 721 } 722 } 723 else if (b==1.) { 724 for (Index i=0; i<Dim(); i++) { 725 values_[i] += a*values_v1[i] + values_v2[i]; 726 } 727 } 728 else if (b==-1.) { 729 for (Index i=0; i<Dim(); i++) { 730 values_[i] += a*values_v1[i] - values_v2[i]; 731 } 732 } 733 else { 734 for (Index i=0; i<Dim(); i++) { 735 values_[i] += a*values_v1[i] + b*values_v2[i]; 736 } 737 } 738 } 739 } 740 else if (c==-1.) { 741 if (a==1.) { 742 if (b==0.) { 743 for (Index i=0; i<Dim(); i++) { 744 values_[i] = values_v1[i] - values_[i]; 745 } 746 } 747 else if (b==1.) { 748 for (Index i=0; i<Dim(); i++) { 749 values_[i] = values_v1[i] + values_v2[i] - values_[i]; 750 } 751 } 752 else if (b==-1.) { 753 for (Index i=0; i<Dim(); i++) { 754 values_[i] = values_v1[i] - values_v2[i] - values_[i]; 755 } 756 } 757 else { 758 for (Index i=0; i<Dim(); i++) { 759 values_[i] = values_v1[i] + b*values_v2[i] - values_[i]; 760 } 761 } 762 } 763 else if (a==-1.) { 764 if (b==0.) { 765 for (Index i=0; i<Dim(); i++) { 766 values_[i] = -values_v1[i] - values_[i]; 767 } 768 } 769 else if (b==1.) { 770 for (Index i=0; i<Dim(); i++) { 771 values_[i] = -values_v1[i] + values_v2[i] - values_[i]; 772 } 773 } 774 else if (b==-1.) { 775 for (Index i=0; i<Dim(); i++) { 776 values_[i] = -values_v1[i] - values_v2[i] - values_[i]; 777 } 778 } 779 else { 780 for (Index i=0; i<Dim(); i++) { 781 values_[i] = -values_v1[i] + b*values_v2[i] - values_[i]; 782 } 783 } 784 } 785 else if (a==0.) { 786 if (b==0.) { 787 for (Index i=0; i<Dim(); i++) { 788 values_[i] *= -1.; 789 } 790 } 791 else if (b==1.) { 792 for (Index i=0; i<Dim(); i++) { 793 values_[i] = values_v2[i] - values_[i]; 794 } 795 } 796 else if (b==-1.) { 797 for (Index i=0; i<Dim(); i++) { 798 values_[i] = -values_v2[i] - values_[i]; 799 } 800 } 801 else { 802 for (Index i=0; i<Dim(); i++) { 803 values_[i] = b*values_v2[i] - values_[i]; 804 } 805 } 806 } 807 else { 808 if (b==0.) { 809 for (Index i=0; i<Dim(); i++) { 810 values_[i] = a*values_v1[i] - values_[i]; 811 } 812 } 813 else if (b==1.) { 814 for (Index i=0; i<Dim(); i++) { 815 values_[i] = a*values_v1[i] + values_v2[i] - values_[i]; 816 } 817 } 818 else if (b==-1.) { 819 for (Index i=0; i<Dim(); i++) { 820 values_[i] = a*values_v1[i] - values_v2[i] - values_[i]; 821 } 822 } 823 else { 824 for (Index i=0; i<Dim(); i++) { 825 values_[i] = a*values_v1[i] + b*values_v2[i] - values_[i]; 826 } 827 } 828 } 829 } 830 else { 831 if (a==1.) { 832 if (b==0.) { 833 for (Index i=0; i<Dim(); i++) { 834 values_[i] = values_v1[i] + c*values_[i]; 835 } 836 } 837 else if (b==1.) { 838 for (Index i=0; i<Dim(); i++) { 839 values_[i] = values_v1[i] + values_v2[i] + c*values_[i]; 840 } 841 } 842 else if (b==-1.) { 843 for (Index i=0; i<Dim(); i++) { 844 values_[i] = values_v1[i] - values_v2[i] + c*values_[i]; 845 } 846 } 847 else { 848 for (Index i=0; i<Dim(); i++) { 849 values_[i] = values_v1[i] + b*values_v2[i] + c*values_[i]; 850 } 851 } 852 } 853 else if (a==-1.) { 854 if (b==0.) { 855 for (Index i=0; i<Dim(); i++) { 856 values_[i] = -values_v1[i] + c*values_[i]; 857 } 858 } 859 else if (b==1.) { 860 for (Index i=0; i<Dim(); i++) { 861 values_[i] = -values_v1[i] + values_v2[i] + c*values_[i]; 862 } 863 } 864 else if (b==-1.) { 865 for (Index i=0; i<Dim(); i++) { 866 values_[i] = -values_v1[i] - values_v2[i] + c*values_[i]; 867 } 868 } 869 else { 870 for (Index i=0; i<Dim(); i++) { 871 values_[i] = -values_v1[i] + b*values_v2[i] + c*values_[i]; 872 } 873 } 874 } 875 else if (a==0.) { 876 if (b==0.) { 877 for (Index i=0; i<Dim(); i++) { 878 values_[i] *= c; 879 } 880 } 881 else if (b==1.) { 882 for (Index i=0; i<Dim(); i++) { 883 values_[i] = values_v2[i] + c*values_[i]; 884 } 885 } 886 else if (b==-1.) { 887 for (Index i=0; i<Dim(); i++) { 888 values_[i] = -values_v2[i] + c*values_[i]; 889 } 890 } 891 else { 892 for (Index i=0; i<Dim(); i++) { 893 values_[i] = b*values_v2[i] + c*values_[i]; 894 } 895 } 896 } 897 else { 898 if (b==0.) { 899 for (Index i=0; i<Dim(); i++) { 900 values_[i] = a*values_v1[i] + c*values_[i]; 901 } 902 } 903 else if (b==1.) { 904 for (Index i=0; i<Dim(); i++) { 905 values_[i] = a*values_v1[i] + values_v2[i] + c*values_[i]; 906 } 907 } 908 else if (b==-1.) { 909 for (Index i=0; i<Dim(); i++) { 910 values_[i] = a*values_v1[i] - values_v2[i] + c*values_[i]; 911 } 912 } 913 else { 914 for (Index i=0; i<Dim(); i++) { 915 values_[i] = a*values_v1[i] + b*values_v2[i] + c*values_[i]; 916 } 917 } 918 } 919 } 920 initialized_=true; 921 } 922 923 Number FracToBoundImpl(const Vector & delta,Number tau) const924 DenseVector::FracToBoundImpl(const Vector& delta, Number tau) const 925 { 926 DBG_ASSERT(Dim()==delta.Dim()); 927 DBG_ASSERT(tau>=0.); 928 const DenseVector* dense_delta = dynamic_cast<const DenseVector*>(&delta); 929 DBG_ASSERT(dense_delta); 930 931 Number alpha = 1.; 932 Number* values_x = values_; 933 Number* values_delta = dense_delta->values_; 934 if (homogeneous_) { 935 if (dense_delta->homogeneous_) { 936 if (dense_delta->scalar_<0.) { 937 alpha = SimTKIpopt::Min(alpha, -tau/dense_delta->scalar_ * scalar_); 938 } 939 } 940 else { 941 for (Index i=0; i<Dim(); i++) { 942 if (values_delta[i]<0.) { 943 alpha = SimTKIpopt::Min(alpha, -tau/values_delta[i] * scalar_); 944 } 945 } 946 } 947 } 948 else { 949 if (dense_delta->homogeneous_) { 950 if (dense_delta->scalar_<0.) { 951 for (Index i=0; i<Dim(); i++) { 952 alpha = SimTKIpopt::Min(alpha, -tau/dense_delta->scalar_ * values_x[i]); 953 } 954 } 955 } 956 else { 957 for (Index i=0; i<Dim(); i++) { 958 if (values_delta[i]<0.) { 959 alpha = SimTKIpopt::Min(alpha, -tau/values_delta[i] * values_x[i]); 960 } 961 } 962 } 963 } 964 965 DBG_ASSERT(alpha>=0.); 966 return alpha; 967 } 968 AddVectorQuotientImpl(Number a,const Vector & z,const Vector & s,Number c)969 void DenseVector::AddVectorQuotientImpl(Number a, const Vector& z, 970 const Vector& s, Number c) 971 { 972 DBG_ASSERT(Dim()==z.Dim()); 973 DBG_ASSERT(Dim()==s.Dim()); 974 const DenseVector* dense_z = dynamic_cast<const DenseVector*>(&z); 975 DBG_ASSERT(dense_z); 976 DBG_ASSERT(dense_z->initialized_); 977 const DenseVector* dense_s = dynamic_cast<const DenseVector*>(&s); 978 DBG_ASSERT(dense_s); 979 DBG_ASSERT(dense_s->initialized_); 980 981 DBG_ASSERT(c==0. || initialized_); 982 bool homogeneous_z = dense_z->homogeneous_; 983 bool homogeneous_s = dense_s->homogeneous_; 984 985 if ((c==0. || homogeneous_) && homogeneous_z && homogeneous_s) { 986 if (c==0.) { 987 scalar_ = a * dense_z->scalar_ / dense_s->scalar_; 988 } 989 else { 990 scalar_ = c * scalar_ + a * dense_z->scalar_ / dense_s->scalar_; 991 } 992 initialized_ = true; 993 homogeneous_ = true; 994 if (values_) { 995 owner_space_->FreeInternalStorage(values_); 996 values_ = NULL; 997 } 998 return; 999 } 1000 1001 // At least one is not homogeneous 1002 // Make sure we have memory to store a non-homogeneous vector 1003 values_allocated(); 1004 1005 Number* values_z = dense_z->values_; 1006 Number* values_s = dense_s->values_; 1007 1008 if (c==0.) { 1009 if (homogeneous_z) { 1010 // then s is not homogeneous 1011 for (Index i=0; i<Dim(); i++) { 1012 values_[i] = a * dense_z->scalar_ / values_s[i]; 1013 } 1014 } 1015 else if (homogeneous_s) { 1016 // then z is not homogeneous 1017 for (Index i=0; i<Dim(); i++) { 1018 values_[i] = values_z[i] * a / dense_s->scalar_; 1019 } 1020 } 1021 else { 1022 for (Index i=0; i<Dim(); i++) { 1023 values_[i] = a * values_z[i] / values_s[i]; 1024 } 1025 } 1026 } 1027 else if (homogeneous_) { 1028 Number val = c*scalar_; 1029 if (homogeneous_z) { 1030 // then s is not homogeneous 1031 for (Index i=0; i<Dim(); i++) { 1032 values_[i] = val + a * dense_z->scalar_ / values_s[i]; 1033 } 1034 } 1035 else if (homogeneous_s) { 1036 // then z is not homogeneous 1037 for (Index i=0; i<Dim(); i++) { 1038 values_[i] = val + values_z[i] * a / dense_s->scalar_; 1039 } 1040 } 1041 else { 1042 for (Index i=0; i<Dim(); i++) { 1043 values_[i] = val + a * values_z[i] / values_s[i]; 1044 } 1045 } 1046 } 1047 else { 1048 // ToDo could distinguish c = 1 1049 if (homogeneous_z) { 1050 if (homogeneous_s) { 1051 for (Index i=0; i<Dim(); i++) { 1052 values_[i] = c*values_[i] + a * dense_z->scalar_/dense_s->scalar_; 1053 } 1054 } 1055 else { 1056 for (Index i=0; i<Dim(); i++) { 1057 values_[i] = c*values_[i] + a * dense_z->scalar_/values_s[i]; 1058 } 1059 } 1060 } 1061 else { 1062 if (homogeneous_s) { 1063 for (Index i=0; i<Dim(); i++) { 1064 values_[i] = c*values_[i] + values_z[i] * a /dense_s->scalar_; 1065 } 1066 } 1067 else { 1068 for (Index i=0; i<Dim(); i++) { 1069 values_[i] = c*values_[i] + a * values_z[i]/values_s[i]; 1070 } 1071 } 1072 } 1073 } 1074 1075 initialized_ = true; 1076 homogeneous_ = false; 1077 } 1078 CopyToPos(Index Pos,const Vector & x)1079 void DenseVector::CopyToPos(Index Pos, const Vector& x) 1080 { 1081 Index dim_x = x.Dim(); 1082 DBG_ASSERT(dim_x+Pos<=Dim()); 1083 const DenseVector* dense_x = dynamic_cast<const DenseVector*>(&x); 1084 DBG_ASSERT(dense_x); 1085 1086 Number* vals = values_allocated(); 1087 homogeneous_ = false; 1088 1089 if (dense_x->homogeneous_) { 1090 IpBlasDcopy(dim_x, &scalar_, 1, vals+Pos, 1); 1091 } 1092 else { 1093 IpBlasDcopy(dim_x, dense_x->values_, 1, vals+Pos, 1); 1094 } 1095 initialized_ = true; 1096 ObjectChanged(); 1097 } 1098 CopyFromPos(Index Pos,Vector & x) const1099 void DenseVector::CopyFromPos(Index Pos, Vector& x) const 1100 { 1101 Index dim_x = x.Dim(); 1102 DBG_ASSERT(dim_x+Pos<=Dim()); 1103 DenseVector* dense_x = dynamic_cast<DenseVector*>(&x); 1104 DBG_ASSERT(dense_x); 1105 DBG_ASSERT(dense_x->homogeneous_); // This might have to made more general 1106 1107 if (homogeneous_) { 1108 IpBlasDcopy(dim_x, &scalar_, 1, dense_x->values_, 1); 1109 } 1110 else { 1111 IpBlasDcopy(dim_x, values_+Pos, 1, dense_x->values_, 1); 1112 } 1113 // We need to tell X that it has changed! 1114 dense_x->ObjectChanged(); 1115 dense_x->initialized_=true; 1116 } 1117 PrintImpl(const Journalist & jnlst,EJournalLevel level,EJournalCategory category,const std::string & name,Index indent,const std::string & prefix) const1118 void DenseVector::PrintImpl(const Journalist& jnlst, 1119 EJournalLevel level, 1120 EJournalCategory category, 1121 const std::string& name, 1122 Index indent, 1123 const std::string& prefix) const 1124 { 1125 jnlst.PrintfIndented(level, category, indent, 1126 "%sDenseVector \"%s\" with %d elements:\n", 1127 prefix.c_str(), name.c_str(), Dim()); 1128 if (initialized_) { 1129 if (homogeneous_) { 1130 jnlst.PrintfIndented(level, category, indent, 1131 "%sHomogeneous vector, all elements have value %23.16e\n", 1132 prefix.c_str(), scalar_); 1133 } 1134 else { 1135 for (Index i=0; i<Dim(); i++) { 1136 jnlst.PrintfIndented(level, category, indent, 1137 "%s%s[%5d]=%23.16e\n", 1138 prefix.c_str(), name.c_str(), i+1, values_[i]); 1139 } 1140 } 1141 } 1142 else { 1143 jnlst.PrintfIndented(level, category, indent, 1144 "%sUninitialized!\n", 1145 prefix.c_str()); 1146 } 1147 } 1148 } // namespace Ipopt 1149