43 #ifndef EIGEN_USE_NEW_STDVECTOR
44 #define EIGEN_USE_NEW_STDVECTOR
45 #endif // EIGEN_USE_NEW_STDVECTOR
50 #include <Eigen/Geometry>
52 #include <Eigen/StdVector>
54 using namespace Eigen;
69 int doBPCG(
int iters,
double tol,
70 vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
71 vector< map<
int,Matrix<double,N,N>, less<int>, aligned_allocator<Matrix<double,N,N> > > > &cols,
79 int doBPCG2(
int iters,
double tol,
80 vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
81 vector< map<
int,Matrix<double,N,N>, less<int>, aligned_allocator<Matrix<double,N,N> > > > &cols,
91 void mMV(vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
92 vector< map<
int,Matrix<double,N,N>, less<int>, aligned_allocator<Matrix<double,N,N> > > > &cols,
97 void mMV2(vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
101 void mD(vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
106 vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > >
vcols;
111 vector< map<
int,Matrix<double,N,N>, less<int>, aligned_allocator<Matrix<double,N,N> > > > &cols,
117 for (
int i=0; i<(int)cols.size(); i++)
119 vout.segment<N>(i*N) = diag[i]*vin.segment<N>(i*N);
121 map<int,Matrix<double,N,N>, less<int>,
122 aligned_allocator<Matrix<double,N,N> > > &col = cols[i];
125 typename map<int,Matrix<double,N,N>, less<int>,
126 aligned_allocator<Matrix<double,N,N > > >::iterator it;
127 for (it = col.begin(); it != col.end(); it++)
129 int ri = (*it).first;
130 const Matrix<double,N,N> &M = (*it).second;
131 vout.segment<N>(i*N) += M.transpose()*vin.segment<N>(ri*N);
132 vout.segment<N>(ri*N) += M*vin.segment<N>(i*N);
150 for (
int i=0; i<(int)diag.size(); i++)
151 vout.segment<N>(i*N) = diag[i]*vin.segment<N>(i*N);
153 for (
int i=0; i<(int)vcind.size(); i++)
157 const Matrix<double,N,N> &M = vcols[i];
158 vout.segment<N>(ri*N) += M*vin.segment<N>(ii*N);
159 vout.segment<N>(ii*N) += M.transpose()*vin.segment<N>(ri*N);
167 void jacobiBPCG<N>::mD(vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
172 for (
int i=0; i<(int)diag.size(); i++)
173 vout.segment<N>(i*N) = diag[i]*vin.segment<N>(i*N);
179 vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
180 vector< map<
int,Matrix<double,N,N>, less<int>, aligned_allocator<Matrix<double,N,N> > > > &cols,
196 vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > J;
198 for (
int i=0; i<n; i++)
199 J[i] = diag[i].inverse();
204 double dn = r.dot(
d);
208 if (residual > d0) d0 = residual;
211 for (i=0; i<iters; i++)
214 cout <<
"[BPCG] residual[" << i <<
"]: " << dn <<
" < " << d0 << endl;
217 double a = dn /
d.dot(q);
224 double ba = dn / dold;
230 cout <<
"[BPCG] residual[" << i <<
"]: " << dn << endl;
239 vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
240 vector< map<
int,Matrix<double,N,N>, less<int>, aligned_allocator<Matrix<double,N,N> > > > &cols,
260 for (
int i=0; i<(int)cols.size(); i++)
262 map<int,Matrix<double,N,N>, less<int>,
263 aligned_allocator<Matrix<double,N,N> > > &col = cols[i];
266 typename map<int,Matrix<double,N,N>, less<int>,
267 aligned_allocator<Matrix<double,N,N> > >::iterator it;
268 for (it = col.begin(); it != col.end(); it++)
270 int ri = (*it).first;
273 vcols.push_back((*it).second);
279 vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > J;
281 for (
int i=0; i<n; i++)
282 J[i] = diag[i].inverse();
287 double dn = r.dot(
d);
291 if (residual > d0) d0 = residual;
294 for (i=0; i<iters; i++)
297 cout <<
"[BPCG] residual[" << i <<
"]: " << dn <<
" < " << d0 << endl;
300 double a = dn /
d.dot(q);
307 double ba = dn / dold;
313 cout <<
"[BPCG] residual[" << i <<
"]: " << dn << endl;
325 mMV(vector< Matrix<double,6,6>, aligned_allocator<Matrix<double,6,6> > > &diag,
326 vector< map<
int,Matrix<double,6,6>, less<int>, aligned_allocator<Matrix<double,6,6> > > > &cols,
335 bpcg_jacobi(
int iters,
double tol,
336 vector< Matrix<double,6,6>, aligned_allocator<Matrix<double,6,6> > > &diag,
337 vector< map<
int,Matrix<double,6,6>, less<int>, aligned_allocator<Matrix<double,6,6> > > > &cols,
345 bpcg_jacobi_dense(
int iters,
double tol,
355 bpcg_jacobi3(
int iters,
double tol,
356 vector< Matrix<double,3,3>, aligned_allocator<Matrix<double,3,3> > > &diag,
357 vector< map<
int,Matrix<double,3,3>, less<int>, aligned_allocator<Matrix<double,3,3> > > > &cols,
365 bpcg_jacobi_dense3(
int iters,
double tol,