27
27
*/
28
28
29
29
template <typename MT, typename VT, typename NT>
30
- void calcstep (MT const & A, MT const & A_trans, MT const & R , VT &s,
30
+ void calcstep (MT const & A, MT const & A_trans, Eigen::LLT<MT> const & lltOfB , VT &s,
31
31
VT &y, VT &r1, VT const & r2, NT const & r3, VT &r4,
32
32
VT &dx, VT &ds, NT &dt, VT &dy, VT &tmp, VT &rhs)
33
33
{
@@ -41,7 +41,8 @@ void calcstep(MT const& A, MT const& A_trans, MT const& R, VT &s,
41
41
42
42
rhs.block (0 ,0 ,n,1 ).noalias () = r2 + A_trans * tmp;
43
43
rhs (n) = r3 + tmp.sum ();
44
- VT dxdt = R.colPivHouseholderQr ().solve (R.transpose ().colPivHouseholderQr ().solve (rhs));
44
+
45
+ VT dxdt = lltOfB.solve (rhs);
45
46
46
47
dx = dxdt.block (0 ,0 ,n,1 );
47
48
dt = dxdt (n);
@@ -57,10 +58,12 @@ void calcstep(MT const& A, MT const& A_trans, MT const& R, VT &s,
57
58
58
59
59
60
template <typename MT, typename VT, typename NT>
60
- std::tuple<VT, NT, bool > max_inscribed_ball (MT const & A, VT const & b, unsigned int maxiter, NT tol)
61
+ std::tuple<VT, NT, bool > max_inscribed_ball (MT const & A, VT const & b,
62
+ unsigned int maxiter, NT tol,
63
+ const bool feasibility_only = false )
61
64
{
62
65
int m = A.rows (), n = A.cols ();
63
- bool converge;
66
+ bool converge = false ;
64
67
65
68
NT bnrm = b.norm ();
66
69
VT o_m = VT::Zero (m), o_n = VT::Zero (n), e_m = VT::Ones (m);
@@ -81,7 +84,7 @@ std::tuple<VT, NT, bool> max_inscribed_ball(MT const& A, VT const& b, unsigned
81
84
NT const tau0 = 0.995 , power_num = 5.0 * std::pow (10.0 , 15.0 );
82
85
NT *vec_iter1, *vec_iter2, *vec_iter3, *vec_iter4;
83
86
84
- MT B (n + 1 , n + 1 ), AtD (n, m), R (n + 1 , n + 1 ),
87
+ MT B (n + 1 , n + 1 ), AtD (n, m),
85
88
eEye = std::pow (10.0 , -14.0 ) * MT::Identity (n + 1 , n + 1 ),
86
89
A_trans = A.transpose ();
87
90
@@ -105,16 +108,17 @@ std::tuple<VT, NT, bool> max_inscribed_ball(MT const& A, VT const& b, unsigned
105
108
total_err = std::max (total_err, rgap);
106
109
107
110
// progress output & check stopping
108
- if (total_err < tol || ( t > 0 && ( (std::abs (t - t_prev) <= tol * t && std::abs (t - t_prev) <= tol * t_prev)
109
- || (t_prev >= (1.0 - tol) * t && i > 0 )
110
- || (t <= (1.0 - tol) * t_prev && i > 0 ) ) ) )
111
+ if ( (total_err < tol && t > 0 ) ||
112
+ ( t > 0 && ( (std::abs (t - t_prev) <= tol * std::min (std::abs (t), std::abs (t_prev)) ||
113
+ std::abs (t - t_prev) <= tol) && i > 10 ) ) ||
114
+ (feasibility_only && t > tol/2.0 && i > 0 ) )
111
115
{
112
116
// converged
113
117
converge = true ;
114
118
break ;
115
119
}
116
120
117
- if (dt > 1000 .0 * bnrm || t > 1000000 .0 * bnrm)
121
+ if (( dt > 10000 .0 * bnrm || t > 10000000 .0 * bnrm) && i > 20 )
118
122
{
119
123
// unbounded
120
124
converge = false ;
@@ -127,11 +131,11 @@ std::tuple<VT, NT, bool> max_inscribed_ball(MT const& A, VT const& b, unsigned
127
131
vec_iter2 = y.data ();
128
132
for (int j = 0 ; j < m; ++j) {
129
133
*vec_iter1 = std::min (power_num, (*vec_iter2) / (*vec_iter3));
130
- AtD.col (j).noalias () = A_trans.col (j) * (*vec_iter1);
131
134
vec_iter1++;
132
135
vec_iter3++;
133
136
vec_iter2++;
134
137
}
138
+ AtD.noalias () = A_trans*d.asDiagonal ();
135
139
136
140
AtDe.noalias () = AtD * e_m;
137
141
B.block (0 , 0 , n, n).noalias () = AtD * A;
@@ -141,11 +145,10 @@ std::tuple<VT, NT, bool> max_inscribed_ball(MT const& A, VT const& b, unsigned
141
145
B.noalias () += eEye;
142
146
143
147
// Cholesky decomposition
144
- Eigen::LLT <MT> lltOfB (B);
145
- R = lltOfB.matrixL ().transpose ();
148
+ Eigen::LLT<MT> lltOfB (B);
146
149
147
150
// predictor step & length
148
- calcstep (A, A_trans, R , s, y, r1, r2, r3, r4, dx, ds, dt, dy, tmp, rhs);
151
+ calcstep (A, A_trans, lltOfB , s, y, r1, r2, r3, r4, dx, ds, dt, dy, tmp, rhs);
149
152
150
153
alphap = -1.0 ;
151
154
alphad = -1.0 ;
@@ -172,7 +175,7 @@ std::tuple<VT, NT, bool> max_inscribed_ball(MT const& A, VT const& b, unsigned
172
175
173
176
// corrector and combined step & length
174
177
mu_ds_dy.noalias () = e_m * mu - ds.cwiseProduct (dy);
175
- calcstep (A, A_trans, R , s, y, o_m, o_n, 0.0 , mu_ds_dy, dxc, dsc, dtc, dyc, tmp, rhs);
178
+ calcstep (A, A_trans, lltOfB , s, y, o_m, o_n, 0.0 , mu_ds_dy, dxc, dsc, dtc, dyc, tmp, rhs);
176
179
177
180
dx += dxc;
178
181
ds += dsc;
0 commit comments