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