00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include "bpcg/bpcg.h"
00039
00040
00041 #include <gtest/gtest.h>
00042
00043 using namespace Eigen;
00044 using namespace sba;
00045
00046 #include <iostream>
00047 #include <fstream>
00048 using namespace std;
00049
00050
00051 TEST(bpcg, mMV)
00052 {
00053
00054
00055
00056
00057 Matrix<double,24,24> M, K, R;
00058 M.setRandom();
00059
00060 for (int i=0; i<4; i++)
00061 for (int j=i; j<4; j++)
00062 {
00063 M.block(i*6,j*6,6,6) = M.block(i*6,j*6,6,6).transpose()*M.block(i*6,j*6,6,6);
00064 M.block(j*6,i*6,6,6) = M.block(i*6,j*6,6,6);
00065 }
00066 M.block(12,0,6,6).setZero();
00067 M.block(0,12,6,6).setZero();
00068
00069 VectorXd v(24), vm(24), vmm(24);
00070 v.setRandom();
00071
00072 vm = M*v;
00073
00074
00075 vector< Matrix<double,6,6>, aligned_allocator<Matrix<double,6,6> > > diag;
00076 vector< map<int,Matrix<double,6,6>, less<int>, aligned_allocator<Matrix<double,6,6> > > > cols;
00077 VectorXd vin(24), vout(24), x0(24);
00078
00079
00080 x0.setZero();
00081 diag.resize(4);
00082 cols.resize(4);
00083 for (int i=0; i<4; i++)
00084 {
00085 int ii = 6*i;
00086 diag[i] = M.block(ii,ii,6,6);
00087
00088 map<int,Matrix<double,6,6>, less<int>,
00089 aligned_allocator<Matrix<double,6,6> > > &col = cols[i];
00090 for (int j=i+1; j<4; j++)
00091 if (!(j==2 && i==0))
00092 col.insert(pair<int,Matrix<double,6,6> >(j,M.block(j*6,ii,6,6)));
00093 }
00094
00095
00096 mMV(diag,cols,v,vout);
00097
00098 cout << v.transpose() << endl;
00099 cout << vout.transpose() << endl;
00100 cout << vm.transpose() << endl;
00101
00102
00103 for (int i=0; i<vm.rows(); i++)
00104 EXPECT_FLOAT_EQ(vm[i],vout[i]);
00105
00106 vin = vout;
00107
00108 #if 1
00109
00110
00111 cols.resize(1);
00112
00113
00114
00115 #endif
00116
00117
00118 MatrixXd MM;
00119 MM.setZero(12,12);
00120 MM << 727624.1239206992,243583.6638335874,136406.3699845557,-649049.2057823476,3177504.726639398,-875433.9245907085,-523918.6146375744,-247000.0715695264,-185935.0054013509,1700939.169375825,-1918392.643847066,-694586.1808429885,
00121 243583.6638335872,257241.5854867174,97918.95660890511,-801347.9323137584,1233703.865879556,-446717.1185187973,-234322.1521862475,-138738.9112753089,-43582.59853617808,800916.1647884385,-912287.2295583673,-294625.2334960104,
00122 136406.3699845563,97918.95660890498,536410.0970080961,-20024.19716237042,875418.3206046737,-74881.80657696929,-152138.6532112983,-111606.5312919594,-507967.6608696034,1054577.587722445,-360152.6926145303,-275341.3454739356,
00123 -649049.2057823451,-801347.9323137584,-20024.19716237064,3735469.787162997,-3622617.915957548,1026576.219219045,713673.2451608701,369642.9319339712,-140694.2692220075,-2816161.328356379,3034044.144042888,1157578.584878814,
00124 3177504.7266394,1233703.865879555,875418.3206046739,-3622617.915957554,15091189.80671031,-3841243.239395392,-2418631.899173797,-1182633.33794011,-1014829.937686714,8629983.77851025,-9282477.133261677,-3223356.600356609,
00125 -875433.9245907082,-446717.1185187968,-74881.80657696963,1026576.219219047,-3841243.239395393,3529022.2517114,515813.4290909402,260815.0170004668,74615.43621058742,-1447359.804337021,2269479.344498967,-794218.0087071136,
00126 -523918.6146375744,-234322.1521862475,-152138.6532112983,713673.2451608701,-2418631.899173797,515813.4290909402,731494.3618955865,236381.8229705717,210817.9916922446,-1831830.813800446,2642745.758829073,1183523.336234943,
00127 -247000.0715695264,-138738.9112753089,-111606.5312919594,369642.9319339712,-1182633.33794011,260815.0170004668,236381.8229705716,235585.179073937,66896.99672782572,-1192988.98662512,845299.8075046183,423528.8050741359,
00128 -185935.0054013509,-43582.59853617808,-507967.6608696034,-140694.2692220075,-1014829.937686714,74615.43621058742,210817.9916922438,66896.99672782577,542230.0471322589,-991900.0725815309,550697.285341059,358713.944182454,
00129 1700939.169375825,800916.1647884385,1054577.587722445,-2816161.328356379,8629983.77851025,-1447359.804337021,-1831830.813800443,-1192988.986625121,-991900.0725815326,8508820.768793903,-6580932.594742717,-3375523.795370819,
00130 -1918392.643847066,-912287.2295583673,-360152.6926145303,3034044.144042888,-9282477.133261677,2269479.344498967,2642745.758829073,845299.8075046184,550697.2853410626,-6580932.594742729,10493795.54564034,3764746.417176266,
00131 -694586.1808429885,-294625.2334960104,-275341.3454739356,1157578.584878814,-3223356.600356609,-794218.0087071136,1183523.336234942,423528.8050741353,358713.9441824536,-3375523.795370816,3764746.417176265,4104760.618614043;
00132
00133 vin = vin.start(12);
00134 vout = MM * vin;
00135 vout << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12;
00136 x0.setZero(12);
00137
00138
00139 bpcg_jacobi_dense(MM.cols()*2,1e-10,MM,x0,vout);
00140 cout << endl;
00141
00142
00143 diag.resize(2);
00144 cols.resize(2);
00145 for (int i=0; i<2; i++)
00146 {
00147 int ii = 6*i;
00148 diag[i] = MM.block(ii,ii,6,6);
00149
00150 map<int,Matrix<double,6,6>, less<int>,
00151 aligned_allocator<Matrix<double,6,6> > > &col = cols[i];
00152 col.clear();
00153 for (int j=i+1; j<2; j++)
00154 col.insert(pair<int,Matrix<double,6,6> >(j,MM.block(j*6,ii,6,6)));
00155 }
00156
00157
00158 x0.setZero(12);
00159 bpcg_jacobi(diag.size()*12,1e-10,diag,cols,x0,vout);
00160
00161
00162 mMV(diag,cols,vin,vout);
00163 x0 = MM*vin;
00164
00165
00166 for (int i=0; i<x0.rows(); i++)
00167 EXPECT_FLOAT_EQ(x0[i],vout[i]);
00168
00169 }
00170
00171
00172
00173 int main(int argc, char **argv){
00174 testing::InitGoogleTest(&argc, argv);
00175 return RUN_ALL_TESTS();
00176 }