1 //JWG
2
3 //------------------------------------------------------------------------
4 // Copyright (C) 1993:
5 // J.C. Meza
6 // Sandia National Laboratories
7 // meza@california.sandia.gov
8 //------------------------------------------------------------------------
9
10 #define WANT_MATH
11
12 #include <iostream>
13 #include <cfloat>
14
15 //#include "include.h"
16
17 #include "Teuchos_SerialDenseMatrix.hpp"
18 #include "Teuchos_SerialDenseVector.hpp"
19 #include "Teuchos_SerialSymDenseMatrix.hpp"
20
21 #define min(a,b) ((a) <= (b) ? (a) : (b))
22 #define max(a,b) ((a) >= (b) ? (a) : (b))
23
24 using Teuchos::SerialSymDenseMatrix;
25 using Teuchos::SerialDenseMatrix;
26
27 namespace OPTPP {
28 //------------------------------------------------------------------------
29 //
30 // Perturbed Cholesky decomposition
31 //
32 //------------------------------------------------------------------------
33
square(double x)34 static double square(double x) { return x*x; }
35 SerialDenseMatrix<int,double> PertChol(SerialSymDenseMatrix<int,double>&, double, double&);
36
MCholesky(SerialSymDenseMatrix<int,double> & S)37 SerialDenseMatrix<int,double> MCholesky(SerialSymDenseMatrix<int,double>& S)
38 {
39 // Tracer trace("MCholesky");
40 using std::sqrt;
41 using std::fabs;
42 int nr = S.numRows();
43 SerialDenseMatrix<int,double> L(nr,nr);
44 double mcheps = DBL_EPSILON;
45
46 // Real* s = S.Store(); Real* l = L.Store();
47
48 double maxadd = 0.0;
49
50 int i, j;
51
52 double sqrteps = sqrt(mcheps);
53 double maxdiag = 0.0;
54 double mindiag = 1.0e10;
55 double maxoff = 0.0;
56 for (i=0; i<nr; ++i) {
57 maxdiag = max(maxdiag,S(i,i));
58 mindiag = min(mindiag,S(i,i));
59 if(i != nr)
60 {for (j=i; j<=i; ++j)
61 {maxoff = max(maxoff,S(i,j));
62 }
63 }
64 }
65
66
67 double maxposdiag = max(0.0,maxdiag);
68 double mu;
69
70 if (mindiag <= sqrteps*maxposdiag) {
71 mu = 2.0*(maxposdiag-mindiag)*sqrteps - mindiag;
72 maxdiag = maxdiag + mu;
73 }
74 else mu = 0.0;
75
76 if (maxoff*(1.0 + 2.0*sqrteps) > maxdiag) {
77 mu = mu + (maxoff-maxdiag) + 2.0*sqrteps*maxoff;
78 maxdiag = maxoff * (1.0 + 2.0*sqrteps);
79 }
80
81 if (maxdiag == 0.0) {
82 mu = 1.0;
83 maxdiag = 1.0;
84 }
85 if (mu > 0.0) {
86 for (i=0; i<nr; ++i) S(i,i) = S(i,i) + mu;
87 }
88
89 double maxoffl = sqrt(max(maxdiag,(maxoff/nr)));
90
91 L = PertChol(S,maxoffl,maxadd);
92
93
94 if (maxadd > 0.0) {
95
96 double maxev = S(0,0);
97 double minev = S(0,0);
98 for (i=0; i<nr; ++i) {
99 double offrow = 0.0;
100 for(j=0; j<=i-1; ++j) offrow += fabs(S(j,i));
101 for(j=i+1; j<nr; ++j) offrow += fabs(S(i,j));
102 maxev = max(maxev,(S(i,i)+offrow));
103 minev = min(minev,(S(i,i)-offrow));
104 }
105 double sdd = (maxev -minev) * sqrteps - minev;
106 sdd = max(sdd,0.0);
107 mu = min(maxadd,sdd);
108 for (i=0; i<nr; ++i) S(i,i) = S(i,i) + mu;
109
110 L = PertChol(S,0.0,maxadd);
111 }
112
113 return L;
114 }
115
116
117
PertChol(SerialSymDenseMatrix<int,double> & S,double maxoffl,double & maxadd)118 SerialDenseMatrix<int,double> PertChol(SerialSymDenseMatrix<int,double>& S, double maxoffl, double& maxadd)
119 {
120 using std::sqrt;
121 using std::fabs;
122 int i;
123 // Tracer trace("PertChol");
124 int nr = S.numRows();
125 SerialDenseMatrix<int,double> L(nr,nr);
126 double mcheps = DBL_EPSILON;
127
128 // Real* s = S.Store(); Real* l = L.Store();
129 double sum;
130 double minl2 = 0.0;
131
132 int j, k;
133 double minl = std::pow(mcheps,.25)*maxoffl;
134
135 if (maxoffl == 0.0) {
136 double maxdiag = 0.0;
137 for (i=0;i<nr;++i) maxdiag = max(maxdiag,fabs(S(i,i)));
138 maxoffl = sqrt(maxdiag);
139 minl2 = sqrt(mcheps)*maxoffl;
140 }
141 maxadd = 0.0;
142
143 for (j=0; j<nr; j++) {
144 sum = 0.0;
145 for (i=0; i<=j-1; ++i) {
146 sum += L(j,i)*L(j,i);
147 }
148 double ljj = S(j,j) - sum;
149
150 double minljj = 0.0;
151
152 for (i=j+1; i<nr; ++i) {
153 sum = 0.0;
154 for(k=0; k<=j-1; ++k) {
155 sum += L(i,k) * L(j,k);
156 }
157 L(i,j) = S(j,i) - sum;
158 minljj = max(fabs(L(i,j)),minljj);
159 }
160 minljj = max((minljj/maxoffl),minl);
161
162 if (ljj > square(minljj)) { // Normal Cholesky
163 L(j,j) = sqrt(ljj);
164 }
165 else {// Modify ljj since it is too small
166 if (minljj < minl2) minljj = minl2;
167 maxadd = max(maxadd,(square(minljj)-ljj));
168 L(j,j) = minljj;
169 }
170 for (i=j+1; i<nr; ++i){
171 L(i,j) = L(i,j) / L(j,j);
172 }
173
174 }
175 return L;
176 }
177
178 } // namespace OPTPP
179
180