00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 template <int Rows, int Cols, typename Base>
00019 _Matrix<Rows,Cols,Base>& _Matrix<Rows, Cols, Base>::operator += (const _Matrix<Rows, Cols, Base>& v){
00020 for (int i=0; i<rows(); i++)
00021 for (int j=0; j<cols(); j++)
00022 _allocator[i][j]+=v._allocator[i][j];
00023 return *this;
00024 }
00025
00026 template <int Rows, int Cols, typename Base>
00027 _Matrix<Rows,Cols,Base>& _Matrix<Rows,Cols,Base>::operator -= (const _Matrix<Rows,Cols,Base>& v){
00028 for (int i=0; i<rows(); i++)
00029 for (int j=0; j<cols(); j++)
00030 _allocator[i][j]-=v._allocator[i][j];
00031 return *this;
00032 }
00033
00034 template <int Rows, int Cols, typename Base>
00035 _Matrix<Rows,Cols,Base> _Matrix<Rows, Cols, Base>::operator - (const _Matrix<Rows, Cols, Base>& v) const {
00036 _Matrix<Rows, Cols, Base> aux(*this);
00037 aux-=v;
00038 return aux;
00039 }
00040
00041 template <int Rows, int Cols, typename Base>
00042 _Matrix<Rows,Cols,Base> _Matrix<Rows, Cols, Base>::operator + (const _Matrix<Rows, Cols, Base>& v) const {
00043 _Matrix<Rows, Cols, Base> aux(*this);
00044 aux+=v;
00045 return aux;
00046 }
00047
00048 template <int Rows, int Cols, typename Base>
00049 _Matrix<Rows,Cols,Base>& _Matrix<Rows,Cols,Base>::operator *= (Base c){
00050 for (int i=0; i<rows(); i++)
00051 for (int j=0; j<cols(); j++)
00052 _allocator[i][j]*=c;
00053 return *this;
00054 }
00055
00056 template <int Rows, int Cols, typename Base>
00057 _Matrix<Rows,Cols,Base> _Matrix<Rows,Cols,Base>::operator * (Base c) const{
00058 _Matrix<Rows,Cols,Base> aux(*this);
00059 aux*=c;
00060 return aux;
00061 }
00062
00063
00064 template <int Rows, int Cols, typename Base>
00065 _Matrix<Cols, Rows, Base> _Matrix<Rows, Cols, Base>::transpose() const{
00066 _Matrix<Cols, Rows, Base> aux(cols(),rows());
00067 for (int i=0; i<rows(); i++)
00068 for (int j=0; j<cols(); j++)
00069 aux._allocator[j][i]=_allocator[i][j];
00070 return aux;
00071 }
00072
00073 template <int Rows, int Cols, typename Base>
00074 _Matrix<Cols, Rows, Base>& _Matrix<Rows, Cols, Base>::transposeInPlace(){
00075 if (rows() == cols()) {
00076 for (int i=0; i<rows(); i++)
00077 for (int j=i+1; j<cols(); j++)
00078 swap((*this)[i][j], (*this)[j][i]);
00079 } else {
00080 _Matrix<Cols, Rows, Base> aux(cols(), rows());
00081 for (int i=0; i<rows(); i++)
00082 for (int j=0; j<cols(); j++)
00083 aux._allocator[j][i]=_allocator[i][j];
00084 *this=aux;
00085 }
00086 return *this;
00087 }
00088
00089
00090
00091 template <int Rows, int Cols, typename Base>
00092 _Matrix<Rows, Rows, Base> _Matrix<Rows, Cols, Base>::eye(Base factor, int size_){
00093 _Matrix<Rows, Rows, Base> aux(size_, size_);
00094 for (int i=0; i<aux.rows(); i++)
00095 for (int j=0; j<aux.cols(); j++)
00096 aux._allocator[i][j]=Base(0);
00097 for (int i=0; i<aux.rows(); i++){
00098 aux._allocator[i][i]=Base(factor);
00099 }
00100 return aux;
00101 }
00102
00103 template <int Rows, int Cols, typename Base>
00104 _Matrix<Rows, Rows, Base> _Matrix<Rows, Cols, Base>::diag(const _Vector<Rows, Base>& v){
00105 _Matrix<Rows, Rows, Base> aux(v.size(), v.size());
00106 for (int i=0; i<aux.rows(); i++)
00107 for (int j=0; j<aux.cols(); j++)
00108 aux._allocator[i][j]=Base(0);
00109 for (int i=0; i<aux.rows(); i++){
00110 aux._allocator[i][i]=v[i];
00111 }
00112 return aux;
00113 }
00114
00115 template <int Rows, int Cols, typename Base>
00116 _Matrix<Rows, Rows, Base> _Matrix<Rows, Cols, Base>::permutation(const _Vector<Rows, int>& p){
00117 _Matrix<Rows, Rows, Base> aux(p.size(), p.size());
00118 for (int i=0; i<aux.rows(); i++)
00119 for (int j=0; j<aux.cols(); j++)
00120 aux._allocator[i][j]=Base(0);
00121 for (int i=0; i<aux.rows(); i++){
00122 aux._allocator[p[i]][i]=p[i];
00123 }
00124 return aux;
00125 }
00126
00127 template <int Rows, int Cols, typename Base>
00128 _Matrix<Rows, Rows, Base> _Matrix<Rows, Cols, Base>::outerProduct(const _Vector<Rows, Base>& v1, const _Vector<Rows, Base>& v2){
00129 assert(v1.size()==v2.size());
00130 _Matrix<Rows, Rows, Base> aux(v1.size(), v1.size());
00131 for (int i=0; i<aux.rows(); i++)
00132 for (int j=0; j<aux.cols(); j++)
00133 aux._allocator[i][j]=v1[i]*v2[j];
00134 return aux;
00135 }
00136
00137
00138
00139 template <int Rows, int Cols, typename Base>
00140 template <int Rows1, int Cols1>
00141 _Matrix<Rows, Cols1, Base> _Matrix<Rows, Cols, Base>::operator*(const _Matrix<Rows1, Cols1, Base>& m) const{
00142 _Matrix<Cols1, Rows1, Base> tm=m.transpose();
00143 _Matrix<Rows, Cols1, Base> aux(rows(),m.cols());
00144 for (int i=0; i<aux.rows(); i++)
00145 for (int j=0; j<aux.cols(); j++){
00146 Base acc(0);
00147 for (int k=0; k<cols(); k++){
00148 acc+=_allocator[i][k]*tm[j][k];
00149 }
00150 aux._allocator[i][j]=acc;
00151 }
00152 return aux;
00153 }
00154
00155 template <int Rows, int Cols, typename Base>
00156 _Matrix<Rows, Cols, Base>& _Matrix<Rows, Cols, Base>::operator*=(const _Matrix<Rows, Cols, Base>& m) {
00157 assert(rows()==cols());
00158 *this=(*this)*m;
00159 return *this;
00160 }
00161
00162
00163
00164 template <int Rows, int Cols, typename Base>
00165 void _Matrix<Rows, Cols, Base>::swapRows(int r1, int r2){
00166 for (int i=0; i<cols(); i++){
00167 Base aux=_allocator[r1][i];
00168 _allocator[r1][i]=_allocator[r2][i];
00169 _allocator[r2][i]=aux;
00170 }
00171 }
00172
00173 template <int Rows, int Cols, typename Base>
00174 void _Matrix<Rows, Cols, Base>::sumRow(int dest, Base destFactor, int src, Base srcFactor){
00175 for (int i=0; i<cols(); i++){
00176 _allocator[dest][i]=_allocator[dest][i]*destFactor+_allocator[src][i]*srcFactor;
00177 }
00178 }
00179
00180 template <int Rows, int Cols, typename Base>
00181 void _Matrix<Rows, Cols, Base>::multRow(int dest, Base destFactor){
00182 for (int i=0; i<cols(); i++){
00183 _allocator[dest][i]*=destFactor;
00184 }
00185 }
00186
00187
00188 template <int Rows, int Cols, typename Base>
00189 _Matrix<Rows, Rows, Base> _Matrix<Rows, Cols, Base>::inverse() const {
00190 assert(rows()==cols() && "Matrix not square");
00191
00192
00193 if(rows()==2){
00194 _Matrix<Rows, Rows, Base> ret(*this);
00195 Base d=det();
00196 if (fabs(d)<=Base(0)){
00197 std::cerr << *this << std::endl;
00198 assert(0 && "Matrix not invertible");
00199 }
00200 ret[0][0]=_allocator[1][1];
00201 ret[1][1]=_allocator[0][0];
00202 ret[1][0]=-_allocator[1][0];
00203 ret[0][1]=-_allocator[0][1];
00204 ret*=(Base(1.)/d);
00205 return ret;
00206 }
00207
00208 _Matrix<Rows, Rows, Base> L(*this);
00209 _Matrix<Rows, Rows, Base> U=_Matrix<Rows, Rows, Base>::eye(Base(1),rows());
00210
00211
00212 for (int i=0;i<rows();i++) {
00213
00214 Base absMax=Base(0);
00215 int k=-1;
00216 for (int q=i;q<rows();q++){
00217 Base absVal=fabs(L._allocator[q][i]);
00218 if (absVal>absMax){
00219 k=q;
00220 absMax=absVal;
00221 }
00222 }
00223
00224 if (k==-1) {
00225 std::cerr << "Matrix not invertible" << std::endl;
00226 std::cerr << *this << std::endl;
00227 assert(0 && "Matrix not invertible");
00228 } else {
00229 L.swapRows(k,i);
00230 U.swapRows(k,i);
00231 }
00232
00233 Base val=L._allocator[i][i];
00234 val=Base(1)/val;
00235 L.multRow(i,val);
00236 U.multRow(i,val);
00237
00238 for (int j=0;j<rows();j++)
00239 if (j!=i) {
00240 Base tmp=-L._allocator[j][i];
00241 L.sumRow(j,Base(1),i,tmp);
00242 U.sumRow(j,Base(1),i,tmp);
00243 }
00244 }
00245 return U;
00246 }
00247
00248 template <int Rows, int Cols, typename Base>
00249 _Matrix<Rows, Rows, Base> _Matrix<Rows, Cols, Base>::cholesky() const {
00250 assert(Rows==Cols);
00251 _Matrix<Rows, Rows, Base> L=_Matrix<Rows, Rows, Base>::eye(0.,rows());
00252 Base diag[rows()];
00253 for (int i=0; i<rows(); i++)
00254 for (int j=0; j<=i; j++){
00255 if (i==j){
00256 Base aux=_allocator[j][j];
00257 for (int k=0; k<j; k++)
00258 aux-=L[i][k]*L[i][k];
00259 assert (aux>Base(0));
00260 L[j][j]=sqrt(aux);
00261 diag[j]=1./L[j][j];
00262 } else {
00263 Base aux=_allocator[i][j];
00264 for (int k=0; k<j; k++)
00265 aux-=L[i][k]*L[j][k];
00266 L[i][j]=aux*diag[j];
00267 }
00268 }
00269 return L;
00270 }
00271
00272 template <int Rows, int Cols, typename Base>
00273 Base _Matrix<Rows, Cols, Base>::det() const {
00274 assert(Rows==Cols);
00275
00276 if (rows()==2 && cols()==2){
00277 return _allocator[0][0]*_allocator[1][1]-_allocator[1][0]*_allocator[0][1];
00278 }
00279
00280
00281 _Matrix<Rows, Rows, Base> aux(*this);
00282 Base d=Base(1);
00283 for (int i=0;i<rows();i++) {
00284 int k=i;
00285 for (;k<Rows&&aux[k][i]==Base(0);k++) {}
00286 if (k>=Rows) return Base(0);
00287 Base val=aux[k][i];
00288 for (int j=0;j<rows();j++) {
00289 aux[k][j]/=val;
00290 }
00291 d=d*val;
00292 if (k!=i) {
00293 for (int j=0;j<rows();j++) {
00294 Base tmp=aux[k][j];
00295 aux[k][j]=aux[i][j];
00296 aux[i][j]=tmp;
00297 }
00298 d=-d;
00299 }
00300 for (int j=i+1;j<rows();j++){
00301 Base tmp=aux[j][i];
00302 if (!(tmp==Base(0)) ){
00303 for (int l=0;l<rows();l++) {
00304 aux[j][l]=aux[j][l]-tmp*aux[i][l];
00305 }
00306 }
00307 }
00308 }
00309 return d;
00310 }
00311
00312 template <int Rows, int Cols, typename Base>
00313 int _Matrix<Rows, Cols, Base>::nullSpace(_Matrix<Rows, Cols, Base>& null_, Base epsilon) const{
00314
00315
00316 _Matrix<Rows, Cols, Base> L(*this);
00317 bool freeVars[rows()];
00318 for (int i=0; i<rows(); i++)
00319 freeVars[i]=false;
00320
00321 int fidx=0;
00322 for (int i=0;i<rows();i++) {
00323
00324 Base absMax=epsilon;
00325 int k=-1;
00326 for (int q=i;q<rows();q++){
00327 Base absVal=fabs(L[q][fidx]);
00328 if (absVal>absMax){
00329 k=q;
00330 absMax=absVal;
00331 }
00332 }
00333
00334
00335 if (k==-1) {
00336 freeVars[fidx]=true;
00337 fidx++;
00338 continue;
00339 } else {
00340 L.swapRows(k,i);
00341 }
00342
00343
00344 L.multRow(i,Base(1)/L._allocator[i][fidx]);
00345 for (int j=0;j<rows();j++)
00346 if (j!=i)
00347 L.sumRow(j,Base(1),i,-L[j][fidx]);
00348 fidx++;
00349 }
00350
00351 while (fidx<cols()){
00352 freeVars[fidx]=true;
00353 fidx++;
00354 }
00355 null_.fill(Base(0));
00356 int fv=0;
00357 for (int i=0; i<cols(); i++){
00358 if (freeVars[i]){
00359 int bv=0;
00360 for (int j=0; j<i; j++){
00361 if (! freeVars[j]){
00362 null_[fv][j]=-L[bv][i];
00363 bv++;
00364 }
00365 }
00366 null_[fv][i]=Base(1);
00367 fv++;
00368 }
00369 }
00370 return fv;
00371 }
00372
00373 template <int R, int C, typename Base>
00374 std::ostream& operator << (std::ostream& os, const _Matrix<R, C, Base>& m){
00375 for (int j=0; j<m.rows(); j++){
00376 if (j > 0)
00377 os << std::endl;
00378 for (int i=0; i<m.cols(); i++){
00379 if (i>0)
00380 os << " ";
00381 os << (m[j][i]);
00382 }
00383 }
00384 return os;
00385 }
00386
00387 template <int Rows, int Cols, typename Base>
00388 _Vector<Rows, Base> _Matrix<Rows, Cols, Base>::operator* (const _Vector<Cols, Base>& v) const{
00389 _Vector<Rows, Base> aux;
00390 for (int i=0; i<rows(); i++){
00391 Base acc=Base(0);
00392 for (int j=0; j<cols(); j++)
00393 acc+=_allocator[i][j]*v[j];
00394 aux[i]=acc;
00395 }
00396 return aux;
00397 }
00398
00399 template <int N, typename Base>
00400 _Vector<N, Base>::operator _Matrix<N, 1, Base> () const{
00401 _Matrix<N, 1, Base> aux;
00402 for(int i=0; i<size(); i++)
00403 aux[0][i]=_allocator[i];
00404 return aux;
00405 }
00406
00407 template <int Rows, int Cols, typename Base>
00408 void _Matrix<Rows, Cols, Base>::fill(Base scalar)
00409 {
00410 for (int j=0; j<rows(); j++)
00411 for (int i=0; i<cols(); i++)
00412 (*this)[j][i] = scalar;
00413 }
00414
00415 template <int R, int C, typename Base>
00416 _Matrix<R, C, Base> operator* (Base x, const _Matrix<R, C, Base>& m)
00417 {
00418 return m * x;
00419 }