961 {
962
963
964
965
966
967
968
969
970 int i, j, k, is;
971 int pivrow;
972
973
974
975
976
977
978
979 static const int max_array = 25;
980#ifdef DISABLE_ALLOC
981 static std::vector<double> xvec (max_array);
982 static std::vector<int> pivv (max_array);
983 typedef std::vector<int>::iterator pivIter;
984#else
985 static std::vector<double,Alloc<double,25> > xvec (max_array);
986 static std::vector<int, Alloc<int, 25> > pivv (max_array);
987 typedef std::vector<int,Alloc<int,25> >::iterator pivIter;
988#endif
989 if (xvec.size() < static_cast<unsigned int>(nrow)) xvec.resize(nrow);
990 if (pivv.size() < static_cast<unsigned int>(nrow)) pivv.resize(nrow);
991
992
993
994
995 mIter x = xvec.begin();
996
997 pivIter piv = pivv.begin();
998
999
1000 double temp1, temp2;
1002 double lambda, sigma;
1003 const double alpha = .6404;
1004 const double epsilon = 32*DBL_EPSILON;
1005
1006
1007
1008
1009
1010 for (i = 0; i < nrow; ++i) piv[i] = i+1;
1011
1012 ifail = 0;
1013
1014
1015
1016
1017
1018 for (j=1; j < nrow; j+=is)
1019 {
1020 mjj = m.begin() + j*(j-1)/2 + j-1;
1021 lambda = 0;
1022 pivrow = j+1;
1023
1024 for (i=j+1; i <= nrow ; ++i) {
1025
1026 ip = m.begin() + (i-1)*i/2 + j-1;
1027 if (fabs(*ip) > lambda) {
1028 lambda = fabs(*ip);
1029 pivrow = i;
1030 }
1031 }
1032 if (lambda == 0 ) {
1033 if (*mjj == 0) {
1034 ifail = 1;
1035 return;
1036 }
1037 is=1;
1038 *mjj = 1./ *mjj;
1039 } else {
1040 if (fabs(*mjj) >= lambda*alpha) {
1041 is=1;
1042 pivrow=j;
1043 } else {
1044 sigma = 0;
1045 ip = m.begin() + pivrow*(pivrow-1)/2+j-1;
1046 for (k=j; k < pivrow; k++) {
1047 if (fabs(*ip) > sigma) sigma = fabs(*ip);
1048 ip++;
1049 }
1050 if (sigma * fabs(*mjj) >= alpha * lambda * lambda) {
1051 is=1;
1052 pivrow = j;
1053 } else if (fabs(*(m.begin()+pivrow*(pivrow-1)/2+pivrow-1))
1054 >= alpha * sigma) {
1055 is=1;
1056 } else {
1057 is=2;
1058 }
1059 }
1060 if (pivrow == j) {
1061 piv[j-1] = pivrow;
1062 if (*mjj == 0) {
1063 ifail=1;
1064 return;
1065 }
1066 temp2 = *mjj = 1./ *mjj;
1067
1068
1069 for (i=j+1; i <= nrow; i++) {
1070 temp1 = *(m.begin() + i*(i-1)/2 + j-1) * temp2;
1071 ip = m.begin()+i*(i-1)/2+j;
1072 for (k=j+1; k<=i; k++) {
1073 *ip -= temp1 * *(m.begin() + k*(k-1)/2 + j-1);
1074 if (fabs(*ip) <= epsilon)
1075 *ip=0;
1076 ip++;
1077 }
1078 }
1079
1080
1081 for (i=j+1; i <= nrow; ++i) {
1082
1083 ip = m.begin() + (i-1)*i/2 + j-1;
1084 *ip *= temp2;
1085 }
1086 } else if (is==1) {
1087 piv[j-1] = pivrow;
1088
1089
1090
1091 ip = m.begin() + pivrow*(pivrow-1)/2 + j;
1092 for (i=j+1; i < pivrow; i++, ip++) {
1093 temp1 = *(m.begin() + i*(i-1)/2 + j-1);
1094 *(m.begin() + i*(i-1)/2 + j-1)= *ip;
1095 *ip = temp1;
1096 }
1097 temp1 = *mjj;
1098 *mjj = *(m.begin()+pivrow*(pivrow-1)/2+pivrow-1);
1099 *(m.begin()+pivrow*(pivrow-1)/2+pivrow-1) = temp1;
1100 ip = m.begin() + (pivrow+1)*pivrow/2 + j-1;
1101 iq = ip + pivrow-j;
1102 for (i = pivrow+1; i <= nrow; ip += i, iq += i++) {
1103 temp1 = *iq;
1104 *iq = *ip;
1105 *ip = temp1;
1106 }
1107
1108 if (*mjj == 0) {
1109 ifail = 1;
1110 return;
1111 }
1112 temp2 = *mjj = 1./ *mjj;
1113
1114
1115 for (i = j+1; i <= nrow; i++) {
1116 temp1 = *(m.begin() + i*(i-1)/2 + j-1) * temp2;
1117 ip = m.begin()+i*(i-1)/2+j;
1118 for (k=j+1; k<=i; k++) {
1119 *ip -= temp1 * *(m.begin() + k*(k-1)/2 + j-1);
1120 if (fabs(*ip) <= epsilon)
1121 *ip=0;
1122 ip++;
1123 }
1124 }
1125
1126
1127 for (i=j+1; i <= nrow; ++i) {
1128
1129 ip = m.begin() + (i-1)*i/2 + j-1;
1130 *ip *= temp2;
1131 }
1132 } else {
1133 piv[j-1] = -pivrow;
1134 piv[j] = 0;
1135
1136 if (j+1 != pivrow) {
1137
1138
1139 ip = m.begin() + pivrow*(pivrow-1)/2 + j+1;
1140 for (i=j+2; i < pivrow; i++, ip++) {
1141 temp1 = *(m.begin() + i*(i-1)/2 + j);
1142 *(m.begin() + i*(i-1)/2 + j) = *ip;
1143 *ip = temp1;
1144 }
1145 temp1 = *(mjj + j + 1);
1146 *(mjj + j + 1) =
1147 *(m.begin() + pivrow*(pivrow-1)/2 + pivrow-1);
1148 *(m.begin() + pivrow*(pivrow-1)/2 + pivrow-1) = temp1;
1149 temp1 = *(mjj + j);
1150 *(mjj + j) = *(m.begin() + pivrow*(pivrow-1)/2 + j-1);
1151 *(m.begin() + pivrow*(pivrow-1)/2 + j-1) = temp1;
1152 ip = m.begin() + (pivrow+1)*pivrow/2 + j;
1153 iq = ip + pivrow-(j+1);
1154 for (i = pivrow+1; i <= nrow; ip += i, iq += i++) {
1155 temp1 = *iq;
1156 *iq = *ip;
1157 *ip = temp1;
1158 }
1159 }
1160
1161 temp2 = *mjj * *(mjj + j + 1) - *(mjj + j) * *(mjj + j);
1162 if (temp2 == 0) {
1163 std::cerr
1164 << "SymMatrix::bunch_invert: error in pivot choice"
1165 << std::endl;
1166 }
1167 temp2 = 1. / temp2;
1168
1169
1170 temp1 = *mjj;
1171 *mjj = *(mjj + j + 1) * temp2;
1172 *(mjj + j + 1) = temp1 * temp2;
1173 *(mjj + j) = - *(mjj + j) * temp2;
1174
1175 if (j < nrow-1) {
1176
1177 for (i=j+2; i <= nrow ; i++) {
1178 ip = m.begin() + i*(i-1)/2 + j-1;
1179 temp1 = *ip * *mjj + *(ip + 1) * *(mjj + j);
1180 if (fabs(temp1 ) <= epsilon) temp1 = 0;
1181 temp2 = *ip * *(mjj + j) + *(ip + 1) * *(mjj + j + 1);
1182 if (fabs(temp2 ) <= epsilon) temp2 = 0;
1183 for (k = j+2; k <= i ; k++) {
1184 ip = m.begin() + i*(i-1)/2 + k-1;
1185 iq = m.begin() + k*(k-1)/2 + j-1;
1186 *ip -= temp1 * *iq + temp2 * *(iq+1);
1187 if (fabs(*ip) <= epsilon)
1188 *ip = 0;
1189 }
1190 }
1191
1192 for (i=j+2; i <= nrow ; i++) {
1193 ip = m.begin() + i*(i-1)/2 + j-1;
1194 temp1 = *ip * *mjj + *(ip+1) * *(mjj + j);
1195 if (fabs(temp1) <= epsilon) temp1 = 0;
1196 *(ip+1) = *ip * *(mjj + j)
1197 + *(ip+1) * *(mjj + j + 1);
1198 if (fabs(*(ip+1)) <= epsilon) *(ip+1) = 0;
1199 *ip = temp1;
1200 }
1201 }
1202 }
1203 }
1204 }
1205
1206 if (j == nrow) {
1207 mjj = m.begin() + j*(j-1)/2 + j-1;
1208 if (*mjj == 0) {
1209 ifail = 1;
1210 return;
1211 } else { *mjj = 1. / *mjj; }
1212 }
1213
1214
1215
1216 for (j = nrow ; j >= 1 ; j -= is)
1217 {
1218 mjj = m.begin() + j*(j-1)/2 + j-1;
1219 if (piv[j-1] > 0) {
1220 is = 1;
1221 if (j < nrow) {
1222
1223
1224 ip = m.begin() + (j+1)*j/2 - 1;
1225 for (i=0; i < nrow-j; ++i) {
1226 ip += i + j;
1227 x[i] = *ip;
1228 }
1229 for (i=j+1; i<=nrow ; i++) {
1230 temp2=0;
1231 ip = m.begin() + i*(i-1)/2 + j;
1232 for (k=0; k <= i-j-1; k++) temp2 += *ip++ * x[k];
1233
1234 ip -= 1;
1235
1236 for ( ; k < nrow-j; ++k) {
1237 ip += j+k;
1238 temp2 += *ip * x[k];
1239 }
1240 *(m.begin()+ i*(i-1)/2 + j-1) = -temp2;
1241 }
1242 temp2 = 0;
1243
1244
1245
1246 ip = m.begin() + (j+1)*j/2 - 1;
1247 for (k=0; k < nrow-j; ++k) {
1248 ip += j+k;
1249 temp2 += x[k] * *ip;
1250 }
1251 *mjj -= temp2;
1252 }
1253 } else {
1254 if (piv[j-1] != 0)
1255 std::cerr << "error in piv" << piv[j-1] << std::endl;
1256 is=2;
1257 if (j < nrow) {
1258
1259
1260 ip = m.begin() + (j+1)*j/2 - 1;
1261 for (i=0; i < nrow-j; ++i) {
1262 ip += i + j;
1263 x[i] = *ip;
1264 }
1265 for (i=j+1; i<=nrow ; i++) {
1266 temp2 = 0;
1267 ip = m.begin() + i*(i-1)/2 + j;
1268 for (k=0; k <= i-j-1; k++)
1269 temp2 += *ip++ * x[k];
1270 for (ip += i-1; k < nrow-j; ip += 1+j+k++)
1271 temp2 += *ip * x[k];
1272 *(m.begin()+ i*(i-1)/2 + j-1) = -temp2;
1273 }
1274 temp2 = 0;
1275
1276
1277 ip = m.begin() + (j+1)*j/2 - 1;
1278 for (k=0; k < nrow-j; ++k) {
1279 ip += k + j;
1280 temp2 += x[k] * *ip;
1281 }
1282 *mjj -= temp2;
1283 temp2 = 0;
1284
1285
1286 ip = m.begin() + (j+1)*j/2 - 2;
1287 for (i=j+1; i <= nrow; ++i) {
1288 ip += i - 1;
1289 temp2 += *ip * *(ip+1);
1290 }
1291 *(mjj-1) -= temp2;
1292
1293
1294 ip = m.begin() + (j+1)*j/2 - 2;
1295 for (i=0; i < nrow-j; ++i) {
1296 ip += i + j;
1297 x[i] = *ip;
1298 }
1299 for (i=j+1; i <= nrow ; i++) {
1300 temp2 = 0;
1301 ip = m.begin() + i*(i-1)/2 + j;
1302 for (k=0; k <= i-j-1; k++)
1303 temp2 += *ip++ * x[k];
1304 for (ip += i-1; k < nrow-j; ip += 1+j+k++)
1305 temp2 += *ip * x[k];
1306 *(m.begin()+ i*(i-1)/2 + j-2)= -temp2;
1307 }
1308 temp2 = 0;
1309
1310
1311
1312 ip = m.begin() + (j+1)*j/2 - 2;
1313 for (k=0; k < nrow-j; ++k) {
1314 ip += k + j;
1315 temp2 += x[k] * *ip;
1316 }
1317 *(mjj-j) -= temp2;
1318 }
1319 }
1320
1321
1322
1323
1324 pivrow = (piv[j-1]==0)? -piv[j-2] : piv[j-1];
1325 ip = m.begin() + pivrow*(pivrow-1)/2 + j;
1326 for (i=j+1;i < pivrow; i++, ip++) {
1327 temp1 = *(m.begin() + i*(i-1)/2 + j-1);
1328 *(m.begin() + i*(i-1)/2 + j-1) = *ip;
1329 *ip = temp1;
1330 }
1331 temp1 = *mjj;
1332 *mjj = *(m.begin() + pivrow*(pivrow-1)/2 + pivrow-1);
1333 *(m.begin() + pivrow*(pivrow-1)/2 + pivrow-1) = temp1;
1334 if (is==2) {
1335 temp1 = *(mjj-1);
1336 *(mjj-1) = *( m.begin() + pivrow*(pivrow-1)/2 + j-2);
1337 *( m.begin() + pivrow*(pivrow-1)/2 + j-2) = temp1;
1338 }
1339
1340
1341 if( pivrow < nrow ) {
1342 ip = m.begin() + (pivrow+1)*pivrow/2 + j-1;
1343
1344 iq = ip + (pivrow-j);
1345 for (i = pivrow+1; i <= nrow; i++) {
1346 temp1 = *iq;
1347 *iq = *ip;
1348 *ip = temp1;
1349 if( i < nrow ) {
1350 ip += i;
1351 iq += i;
1352 }
1353 }
1354 }
1355 }
1356
1357 return;
1358
1359}