fMatrix.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * The University of Tokyo
8  */
16 #include "fMatrix.h"
17 
21 fMat::fMat(const fVec& ini)
22 {
23  p_data = 0;
24  n_col = 0;
25  n_row = 0;
26  m_info = 0;
27  int k = ini.size();
28  resize(k, 1);
29  dims_copy(ini.p_data, p_data, k);
30 }
31 
32 void fMat::get_submat(int row_start, int col_start, const fMat& allM)
33 {
34  assert(row_start >= 0 && col_start >= 0 &&
35  row_start+n_row <= allM.row() &&
36  col_start+n_col <= allM.col());
37  for(int i=0; i<n_col; i++)
38  {
39  double* m = allM.data() + allM.row()*(i+col_start) + row_start;
40  double* p = p_data + n_row*i;
41  for(int j=0; j<n_row; j++, p++, m++)
42  {
43  *p = *m;
44  }
45  }
46 }
47 
48 void fMat::set_submat(int row_start, int col_start, const fMat& subM)
49 {
50  int row_span = subM.row(), col_span = subM.col();
51  assert(row_start >= 0 && col_start >= 0 &&
52  row_start+row_span <= n_row &&
53  col_start+col_span <= n_col);
54  for(int i=0; i<col_span; i++)
55  {
56  double* m = subM.data() + i*subM.row();
57  double* p = p_data + n_row*(i+col_start) + row_start;
58  for(int j=0; j<row_span; j++, p++, m++)
59  {
60  *p = *m;
61  }
62  }
63 }
64 
65 void fVec::get_subvec(int start, const fVec& allV)
66 {
67  assert(start >= 0 && start+n_row <= allV.size());
68  double* m = allV.data() + start;
69  double* p = p_data;
70  for(int i=0; i<n_row; i++, p++, m++)
71  {
72  *p = *m;
73  }
74 }
75 
76 void fVec::set_subvec(int start, const fVec& subV)
77 {
78  int row_span = subV.size();
79  assert(start >= 0 && start+row_span <= n_row);
80  double* m = subV.data();
81  double* p = p_data + start;
82  for(int i=0; i<row_span; i++, p++, m++)
83  {
84  *p = *m;
85  }
86 }
87 
88 void fMat::symmetric(char t)
89 {
90  assert(n_row == n_col);
91  double *p1, *p2;
92  if(t == 'A')
93  {
94  // use average
95  double sum;
96  for(int i=0; i<n_row; i++)
97  {
98  int j;
99  for(j=0, p1=p_data+i, p2=p_data+n_row*i; j<i; j++, p1+=n_row, p2++)
100  {
101  sum = *p1 + *p2;
102  sum *= 0.5;
103  *p1 = sum;
104  *p2 = sum;
105  }
106  }
107  }
108  else if(t == 'U')
109  {
110  // use upper triangle
111  for(int i=0; i<n_row; i++)
112  {
113  int j;
114  for(j=0, p1=p_data+i, p2=p_data+n_row*i; j<i; j++, p1+=n_row, p2++)
115  *p1 = *p2;
116  }
117  }
118  else if(t == 'L')
119  {
120  // use lower triangle
121  for(int i=0; i<n_row; i++)
122  {
123  int j;
124  for(j=0, p1=p_data+i, p2=p_data+n_row*i; j<i; j++, p1+=n_row, p2++)
125  *p2 = *p1;
126  }
127  }
128  else
129  {
130  assert(0);
131  }
132 }
133 
134 /*
135  * LU decomposition
136  */
137 fMat lineq(const fMat& A, const fMat& b)
138 {
139  assert(A.row() == b.row());
140  fMat x(A.col(), b.col());
141  double *a, *bb, *xx;
142  a = A.data();
143  bb = b.data();
144  xx = x.data();
145  int n = A.row(), nrhs = b.col();
146  x.m_info = dims_dgesvx(a, xx, bb, n, nrhs);
147  return x;
148 }
149 
150 int fMat::lineq(const fMat& A, const fMat& b)
151 {
152  assert(n_row == A.col() && n_col == b.col() && A.row() == b.row());
153  double *a, *bb, *xx;
154  int ret;
155  a = A.data();
156  bb = b.data();
157  xx = p_data;
158  int n = A.row(), nrhs = b.col();
159  ret = dims_dgesvx(a, xx, bb, n, nrhs);
160  return ret;
161 }
162 
163 int fVec::lineq(const fMat& A, const fVec& b)
164 {
165  assert(n_row == A.col() && A.row() == b.row());
166  double *a, *bb, *xx;
167  int ret;
168  a = A.data();
169  bb = b.data();
170  xx = p_data;
171  int n = A.row(), nrhs = 1;
172  ret = dims_dgesvx(a, xx, bb, n, nrhs);
173  return ret;
174 }
175 
176 fMat inv(const fMat& mat)
177 {
178  assert(mat.row() == mat.col());
179  fMat ret(mat.row(), mat.col());
180  double *a, *b, *x;
181  int n = mat.row(), nrhs = mat.row();
182  fMat I(mat.row(), mat.col());
183  I.identity();
184  a = mat.data();
185  x = ret.data();
186  b = I.data();
187  ret.m_info = dims_dgesvx(a, x, b, n, nrhs);
188  return ret;
189 }
190 
191 fMat p_inv(const fMat& mat)
192 {
193  fMat ret(mat.col(), mat.row());
194  fMat tmat;
195  fMat MM, MMinv;
196 
197  if(mat.row() < mat.col())
198  {
199  tmat.resize(mat.n_col, mat.n_row);
200  tmat.tran(mat);
201  MM.resize(mat.row(), mat.row());
202  MMinv.resize(mat.row(), mat.row());
203  MM.mul(mat, tmat);
204  MMinv.inv(MM);
205  ret.mul(tmat, MMinv);
206  ret.m_info = MMinv.m_info;
207  }
208  else if(mat.row() > mat.col())
209  {
210  tmat.resize(mat.n_col, mat.n_row);
211  tmat.tran(mat);
212  MM.resize(mat.col(), mat.col());
213  MMinv.resize(mat.col(), mat.col());
214  MM.mul(tmat, mat);
215  MMinv.inv(MM);
216  ret.mul(MMinv, tmat);
217  ret.m_info = MMinv.m_info;
218  }
219  else
220  {
221  ret.inv(mat);
222  }
223  return ret;
224 }
225 
226 fMat sr_inv(const fMat& mat, fVec& w_err, fVec& w_norm, double k)
227 {
228  assert(w_err.row() == mat.row() && w_norm.row() == mat.col());
229  fMat ret(mat.n_col, mat.n_row);
230 
231  fMat Jhat, tJhat;
232  Jhat.resize(mat.row(), mat.col());
233  tJhat.resize(mat.col(), mat.row());
234  int i, j, r = mat.n_row, c = mat.n_col;
235  Jhat.set(mat);
236  for(i=0; i<r; i++)
237  {
238  if(w_err(i) < 1e-8)
239  {
240  w_err(i) = 1.0;
241  }
242  for(j=0; j<c; j++)
243  {
244  Jhat(i, j) *= w_err(i);
245  }
246  }
247  for(j=0; j<c; j++)
248  {
249  if(w_norm(j) < 1e-8)
250  {
251  w_norm(j) = 1.0;
252  }
253  for(i=0; i<r; i++)
254  {
255  Jhat(i, j) /= w_norm(j);
256  }
257  }
258  tJhat.tran(Jhat);
259  static fMat tmp, minv;
260  if(mat.n_row < mat.n_col)
261  {
262  tmp.resize(mat.n_row, mat.n_row);
263  minv.resize(mat.n_row, mat.n_row);
264  tmp.mul(Jhat, tJhat);
265  for(i=0; i<mat.n_row; i++)
266  {
267  tmp(i, i) += k;
268  }
269  minv.inv(tmp);
270  ret.mul(tJhat, minv);
271  ret.m_info = minv.m_info;
272  }
273  else
274  {
275  tmp.resize(mat.n_col, mat.n_col);
276  minv.resize(mat.n_col, mat.n_col);
277  tmp.mul(tJhat, Jhat);
278  for(i=0; i<mat.n_col; i++)
279  {
280  tmp(i, i) += k;
281  }
282  minv.inv(tmp);
283  ret.mul(minv, tJhat);
284  ret.m_info = minv.m_info;
285  }
286  for(i=0; i<mat.n_col; i++)
287  {
288  for(j=0; j<mat.n_row; j++)
289  {
290  ret(i, j) *= w_err(j) / w_norm(i);
291  }
292  }
293  return ret;
294 }
295 
296 int fMat::inv(const fMat& mat)
297 {
298  assert(n_row == n_col && n_row == mat.n_row && n_col == mat.n_col);
299  double *a, *b, *x;
300  int n = mat.row(), nrhs = mat.row();
301  fMat I(mat.row(), mat.col());
302  I.identity();
303  a = mat.data();
304  x = p_data;
305  b = I.data();
306  {
307  m_info = dims_dgesvx(a, x, b, n, nrhs);
308  }
309  return m_info;
310 }
311 
312 int fMat::p_inv(const fMat& mat)
313 {
314  assert(n_row == mat.n_col && n_col == mat.n_row);
315  fMat tmat;
316  fMat MM, MMinv;
317 
318  if(mat.row() < mat.col())
319  {
320  tmat.resize(mat.n_col, mat.n_row);
321  tmat.tran(mat);
322  MM.resize(mat.row(), mat.row());
323  MMinv.resize(mat.row(), mat.row());
324  MM.mul_tran(mat, false);
325  MMinv.inv(MM);
326  mul(tmat, MMinv);
327  m_info = MMinv.m_info;
328  }
329  else if(mat.row() > mat.col())
330  {
331  tmat.resize(mat.n_col, mat.n_row);
332  tmat.tran(mat);
333  MM.resize(mat.col(), mat.col());
334  MMinv.resize(mat.col(), mat.col());
335  MM.mul_tran(mat, true);
336  MMinv.inv(MM);
337  mul(MMinv, tmat);
338  m_info = MMinv.m_info;
339  }
340  else inv(mat);
341  return m_info;
342 }
343 
344 int fMat::sr_inv(const fMat& mat, fVec& w_err, fVec& w_norm, double k)
345 {
346  assert(n_col == mat.n_row && n_row == mat.n_col &&
347  w_err.row() == mat.row() && w_norm.row() == mat.col());
348  fMat Jhat, tJhat;
349  Jhat.resize(mat.row(), mat.col());
350  tJhat.resize(mat.col(), mat.row());
351  int i, j, r = mat.n_row, c = mat.n_col;
352  Jhat.set(mat);
353  for(i=0; i<r; i++)
354  {
355  if(w_err(i) < 1e-8)
356  {
357  w_err(i) = 1.0;
358  }
359  for(j=0; j<c; j++)
360  {
361  Jhat(i, j) *= w_err(i);
362  }
363  }
364  for(j=0; j<c; j++)
365  {
366  if(w_norm(j) < 1e-8)
367  {
368  w_norm(j) = 1.0;
369  }
370  for(i=0; i<r; i++)
371  {
372  Jhat(i, j) /= w_norm(j);
373  }
374  }
375  tJhat.tran(Jhat);
376  fMat tmp, minv;
377  if(mat.n_row < mat.n_col)
378  {
379  tmp.resize(mat.n_row, mat.n_row);
380  minv.resize(mat.n_row, mat.n_row);
381  tmp.mul(Jhat, tJhat);
382  for(i=0; i<mat.n_row; i++)
383  {
384  tmp(i, i) += k;
385  }
386  minv.inv(tmp);
387  mul(tJhat, minv);
388  m_info = minv.m_info;
389  }
390  else
391  {
392  tmp.resize(mat.n_col, mat.n_col);
393  minv.resize(mat.n_col, mat.n_col);
394  tmp.mul(tJhat, Jhat);
395  for(i=0; i<mat.n_col; i++)
396  {
397  tmp(i, i) += k;
398  }
399  minv.inv(tmp);
400  mul(minv, tJhat);
401  m_info = minv.m_info;
402  }
403  for(i=0; i<mat.n_col; i++)
404  {
405  for(j=0; j<mat.n_row; j++)
406  {
407  (*this)(i, j) *= w_err(j) / w_norm(i);
408  }
409  }
410  return m_info;
411 }
412 
413 /*
414  * SVD
415  */
416 fMat lineq_svd(const fMat& A, const fMat& b, int lwork)
417 {
418  assert(A.row() == b.row());
419  fMat x(A.col(), b.col());
420  double *a, *bb, *xx, *s;
421  a = A.data();
422  bb = b.data();
423  xx = x.data();
424  int m = A.row(), n = A.col(), nrhs = b.col(), rank;
425  if(m < n) s = new double[m];
426  else s = new double[n];
427  dims_dgelss(a, xx, bb, m, n, nrhs, s, &rank, lwork);
428  delete[] s;
429  return x;
430 }
431 
432 int fMat::lineq_svd(const fMat& A, const fMat& b, int lwork)
433 {
434  assert(n_row == A.col() && n_col == b.col() && A.row() == b.row());
435  if(A.col() == 1)
436  {
437  fMat AA(1,1);
438  fMat Ainv(A.col(), A.row());
439  AA.mul_tran(A, true);
440  Ainv.tran(A);
441  Ainv /= AA(0,0);
442  mul(Ainv, b);
443  return 0;
444  }
445  else if(A.row() == 1)
446  {
447  fMat AA(1,1);
448  fMat Ainv(A.col(), A.row());
449  AA.mul_tran(A, false);
450  Ainv.tran(A);
451  Ainv /= AA(0,0);
452  mul(Ainv, b);
453  return 0;
454  }
455  double *a, *bb, *xx, *s;
456  int ret;
457  a = A.data();
458  bb = b.data();
459  xx = p_data;
460  int m = A.row(), n = A.col(), nrhs = b.col(), rank;
461  if(m<n) s = new double[m];
462  else s = new double[n];
463  ret = dims_dgelss(a, xx, bb, m, n, nrhs, s, &rank, lwork);
464  delete[] s;
465  return ret;
466 }
467 
468 int fVec::lineq_svd(const fMat& A, const fVec& b, int lwork)
469 {
470  assert(n_row == A.col() && A.row() == b.row());
471  double *a, *bb, *xx, *s;
472  int ret;
473  a = A.data();
474  bb = b.data();
475  xx = p_data;
476  int m = A.row(), n = A.col(), nrhs = 1, rank;
477  if(m<n) s = new double[m];
478  else s = new double[n];
479  ret = dims_dgelss(a, xx, bb, m, n, nrhs, s, &rank, lwork);
480  delete[] s;
481  return ret;
482 }
483 
484 fMat inv_svd(const fMat& mat, int lwork)
485 {
486  assert(mat.row() == mat.col());
487  fMat ret(mat.row(), mat.col());
488  double *a, *b, *x, *s;
489  int m = mat.row(), n = mat.col(), nrhs = mat.row(), rank;
490  fMat I(mat.row(), mat.col());
491  I.identity();
492  a = mat.data();
493  x = ret.data();
494  b = I.data();
495  s = new double[n];
496  ret.m_info = dims_dgelss(a, x, b, m, n, nrhs, s, &rank, lwork);
497  delete[] s;
498  return ret;
499 }
500 
501 fMat p_inv_svd(const fMat& mat, int lwork)
502 {
503  fMat ret(mat.col(), mat.row());
504  static fMat tmat;
505  static fMat MM, MMinv;
506 
507  if(mat.row() < mat.col())
508  {
509  tmat.resize(mat.n_col, mat.n_row);
510  tmat.tran(mat);
511  MM.resize(mat.row(), mat.row());
512  MMinv.resize(mat.row(), mat.row());
513  MM.mul(mat, tmat);
514  MMinv.inv_svd(MM, lwork);
515  ret.mul(MMinv, tmat);
516  ret.m_info = MMinv.m_info;
517  }
518  else if(mat.row() > mat.col())
519  {
520  tmat.resize(mat.n_col, mat.n_row);
521  tmat.tran(mat);
522  MM.resize(mat.col(), mat.col());
523  MMinv.resize(mat.col(), mat.col());
524  MM.mul(tmat, mat);
525  MMinv.inv_svd(MM, lwork);
526  ret.mul(tmat, MMinv);
527  ret.m_info = MMinv.m_info;
528  }
529  else
530  {
531  ret.inv_svd(mat, lwork);
532  }
533  return ret;
534 }
535 
536 fMat sr_inv_svd(const fMat& mat, fVec& w_err, fVec& w_norm, double k, int lwork)
537 {
538  assert(w_err.row() == mat.row() && w_norm.row() == mat.col());
539  fMat ret(mat.n_col, mat.n_row);
540  static fMat Jhat, tJhat;
541  Jhat.resize(mat.row(), mat.col());
542  tJhat.resize(mat.col(), mat.row());
543  int i, j;
544  Jhat.set(mat);
545  for(i=0; i<mat.n_row; i++)
546  {
547  if(w_err(i) < 1e-8)
548  {
549  w_err(i) = 1.0;
550  }
551  for(j=0; j<mat.n_col; j++)
552  {
553  Jhat(i, j) *= w_err(i);
554  }
555  }
556  for(j=0; j<mat.n_col; j++)
557  {
558  if(w_norm(j) < 1e-8)
559  {
560  w_norm(j) = 1.0;
561  }
562  for(i=0; i<mat.n_row; i++)
563  {
564  Jhat(i, j) *= w_norm(j);
565  }
566  }
567  tJhat.tran(Jhat);
568  static fMat tmp, minv;
569  if(mat.n_row < mat.n_col)
570  {
571  tmp.resize(mat.n_row, mat.n_row);
572  minv.resize(mat.n_row, mat.n_row);
573  tmp.mul(Jhat, tJhat);
574  for(i=0; i<mat.n_row; i++)
575  {
576  tmp(i, i) += k;
577  }
578  minv.inv_svd(tmp, lwork);
579  ret.mul(tJhat, minv);
580  ret.m_info = minv.m_info;
581  }
582  else
583  {
584  tmp.resize(mat.n_col, mat.n_col);
585  minv.resize(mat.n_col, mat.n_col);
586  tmp.mul(tJhat, Jhat);
587  for(i=0; i<mat.n_col; i++)
588  {
589  tmp(i, i) += k;
590  }
591  minv.inv_svd(tmp, lwork);
592  ret.mul(minv, tJhat);
593  ret.m_info = minv.m_info;
594  }
595  for(i=0; i<mat.n_col; i++)
596  {
597  for(j=0; j<mat.n_row; j++)
598  {
599  ret(i, j) *= w_err(j) / w_norm(i);
600  }
601  }
602  return ret;
603 }
604 
605 int fMat::inv_svd(const fMat& mat, int lwork)
606 {
607  assert(n_row == mat.n_row && n_col == mat.n_col);
608  if(n_col == 1)
609  {
610  p_data[0] = 1/mat(0, 0);
611  m_info = 0;
612  return m_info;
613  }
614  double *a, *b, *x, *s;
615  int m = mat.row(), n = mat.col(), nrhs = mat.row(), rank;
616  fMat I(mat.row(), mat.col());
617  I.identity();
618  a = mat.data();
619  x = p_data;
620  b = I.data();
621  if(m<n) s = new double[m];
622  else s = new double[n];
623  m_info = dims_dgelss(a, x, b, m, n, nrhs, s, &rank, lwork);
624  delete[] s;
625  return m_info;
626 }
627 
628 int fMat::p_inv_svd(const fMat& mat, int lwork)
629 {
630  assert(n_row == mat.n_col && n_col == mat.n_row);
631  fMat tmat;
632  fMat MM, MMinv;
633 
634  if(mat.row() < mat.col())
635  {
636  tmat.resize(mat.n_col, mat.n_row);
637  tmat.tran(mat);
638  MM.resize(mat.row(), mat.row());
639  MMinv.resize(mat.row(), mat.row());
640 // MM.mul(mat, tmat);
641  MM.mul_tran(mat, false);
642  MMinv.inv_svd(MM, lwork);
643  mul(tmat, MMinv);
644  m_info = MMinv.m_info;
645  }
646  else if(mat.row() > mat.col())
647  {
648  tmat.resize(mat.n_col, mat.n_row);
649  tmat.tran(mat);
650  MM.resize(mat.col(), mat.col());
651  MMinv.resize(mat.col(), mat.col());
652 // MM.mul(tmat, mat);
653  MM.mul_tran(mat, true);
654  MMinv.inv_svd(MM, lwork);
655  mul(MMinv, tmat);
656  m_info = MMinv.m_info;
657  }
658  else inv_svd(mat, lwork);
659  return m_info;
660 }
661 
662 int fMat::sr_inv_svd(const fMat& mat, fVec& w_err, fVec& w_norm, double k, int lwork)
663 {
664  assert(n_col == mat.n_row && n_row == mat.n_col &&
665  w_err.row() == mat.row() && w_norm.row() == mat.col());
666  fMat Jhat, tJhat;
667  Jhat.resize(mat.row(), mat.col());
668  tJhat.resize(mat.col(), mat.row());
669  int i, j;
670  Jhat.set(mat);
671  for(i=0; i<mat.n_row; i++)
672  {
673  if(w_err(i) < 1e-8)
674  {
675  w_err(i) = 1.0;
676  }
677  for(j=0; j<mat.n_col; j++)
678  {
679  Jhat(i, j) *= w_err(i);
680  }
681  }
682  for(j=0; j<mat.n_col; j++)
683  {
684  if(w_norm(j) < 1e-8)
685  {
686  w_norm(j) = 1.0;
687  }
688  for(i=0; i<mat.n_row; i++)
689  {
690  Jhat(i, j) *= w_norm(j);
691  }
692  }
693  tJhat.tran(Jhat);
694  fMat tmp, minv;
695  if(mat.n_row < mat.n_col)
696  {
697  tmp.resize(mat.n_row, mat.n_row);
698  minv.resize(mat.n_row, mat.n_row);
699 // tmp.mul_tran(Jhat, false);
700  tmp.mul(Jhat, tJhat);
701  for(i=0; i<mat.n_row; i++)
702  {
703  tmp(i, i) += k;
704  }
705  minv.inv_svd(tmp, lwork);
706  mul(tJhat, minv);
707  m_info = minv.m_info;
708  }
709  else
710  {
711  tmp.resize(mat.n_col, mat.n_col);
712  minv.resize(mat.n_col, mat.n_col);
713 // tmp.mul_tran(Jhat, true);
714  tmp.mul(tJhat, Jhat);
715  for(i=0; i<mat.n_col; i++)
716  {
717  tmp(i, i) += k;
718  }
719  minv.inv_svd(tmp, lwork);
720  mul(minv, tJhat);
721  m_info = minv.m_info;
722  }
723  for(i=0; i<mat.n_col; i++)
724  {
725  for(j=0; j<mat.n_row; j++)
726  {
727  (*this)(i, j) *= w_err(j) / w_norm(i);
728  }
729  }
730  return m_info;
731 }
732 
733 int fMat::svd(fMat& U, fVec& Sigma, fMat& VT)
734 {
735  assert(n_row == U.n_row && n_row == U.n_col &&
736  Sigma.n_row == MIN(n_row, n_col) &&
737  n_col == VT.n_row && n_col == VT.n_col);
738  double* a = p_data;
739  int ret = dims_svd(a, n_row, n_col, U.data(), Sigma.data(), VT.data());
740  return ret;
741 }
742 
743 /*
744  * POSV
745  */
746 int fMat::lineq_posv(const fMat& A, const fMat& b)
747 {
748  int m = A.row();
749  int nrhs = b.col();
750 // m_info = dims_dporfs(A.data(), p_data, b.data(), m, nrhs);
751  m_info = dims_dposv(A.data(), p_data, b.data(), m, nrhs);
752 // double rcond;
753 // m_info = dims_dposvx(A.data(), p_data, b.data(), m, nrhs, &rcond);
754  return m_info;
755 }
756 
757 int fMat::inv_posv(const fMat& A)
758 {
759  fMat I(A.row(), A.col());
760  I.identity();
761  m_info = lineq_posv(A, I);
762  return m_info;
763 }
764 
765 int fVec::lineq_posv(const fMat& A, const fVec& b)
766 {
767  int m = A.row();
768  m_info = dims_dposv(A.data(), p_data, b.data(), m, 1);
769  return m_info;
770 }
771 
772 int fMat::lineq_porfs(const fMat& A, const fMat& b)
773 {
774  int m = A.row();
775  int nrhs = b.col();
776  m_info = dims_dporfs(A.data(), p_data, b.data(), m, nrhs);
777  return m_info;
778 }
779 
780 int fMat::inv_porfs(const fMat& A)
781 {
782  fMat I(A.row(), A.col());
783  I.identity();
784  m_info = lineq_porfs(A, I);
785  return m_info;
786 }
787 
788 int fVec::lineq_porfs(const fMat& A, const fVec& b)
789 {
790  int m = A.row();
791  m_info = dims_dporfs(A.data(), p_data, b.data(), m, 1);
792  return m_info;
793 }
794 
795 /*
796  * SR-inverse
797  */
798 int fMat::lineq_sr(const fMat& A, fVec& w_err, fVec& w_norm, double k, const fMat& b){
799  assert(n_col == b.col() && n_row == A.col() &&
800  w_err.row() == A.row() && w_norm.row() == A.col() &&
801  b.row() == A.row());
802  fMat Jhat, tJhat, wb;
803  int A_row = A.row(), A_col = A.col(), n_rhs = b.col();
804  Jhat.resize(A_row, A_col);
805  tJhat.resize(A_col, A_row);
806  wb.resize(A_row, n_rhs);
807  int i, j;
808  Jhat.set(A);
809  wb.set(b);
810  for(i=0; i<A_row; i++)
811  {
812  for(j=0; j<A_col; j++)
813  {
814  Jhat(i, j) *= w_err(i);
815  Jhat(i, j) /= w_norm(j);
816  }
817  }
818  tJhat.tran(Jhat);
819  for(i = 0; i < A_row; i++)
820  {
821  for(j = 0; j < n_rhs; j++)
822  {
823  wb(i, j) *= w_err(i);
824  }
825  }
826  fMat tmp, tmpx, tmpb;
827  if(A_row < A_col)
828  {
829  tmpx.resize(A_row, n_rhs);
830  tmp.resize(A_row, A_row);
831  tmp.mul_tran(Jhat, false);
832  for(i=0; i<A_row; i++)
833  {
834  tmp(i, i) += k;
835  }
836  tmpx.lineq_posv(tmp, wb);
837  mul(tJhat, tmpx);
838  m_info = 0; // ???
839  for(i=0; i<A_col; i++)
840  {
841  for(j = 0; j < n_rhs; j++){
842  (*this)(i, j) /= w_norm(i);
843  }
844  }
845  }
846  else
847  {
848  tmp.resize(A_col, A_col);
849  tmpb.resize(A_col, n_rhs);
850  tmp.mul_tran(Jhat, true);
851  for(i=0; i<A_col; i++)
852  {
853  tmp(i, i) += k;
854  }
855  tmpb.mul(tJhat, wb);
856  (*this).lineq_posv(tmp, tmpb);
857  m_info = 0; // ???
858  for(i = 0; i < A_col; i++)
859  {
860  for(j = 0; j < n_rhs; j++)
861  {
862  (*this)(i, j) /= w_norm(i);
863  }
864  }
865  }
866  return m_info;
867 }
868 
869 int fVec::lineq_sr(const fMat& A, fVec& w_err, fVec& w_norm, double k, const fVec& b)
870 {
871  assert(n_row == A.col() && w_err.row() == A.row() &&
872  w_norm.row() == A.col() && b.row() == A.row());
873  fMat Jhat;
874  fVec wb;
875  int A_row = A.row(), A_col = A.col();
876  Jhat.resize(A_row, A_col);
877  wb.resize(A_row);
878  int i, j;
879  Jhat.set(A);
880  wb.set(b);
881  double* pp;
882  for(i=0; i<A_row; i++)
883  {
884  for(j=0; j<A_col; j++)
885  {
886  pp = Jhat.data() + i + A_row*j;
887  *pp *= w_err(i) / w_norm(j);
888  }
889  wb(i) *= w_err(i);
890  }
891  fMat tmp;
892  fVec tmpx, tmpb;
893  if(A_row < A_col)
894  {
895  tmpx.resize(A_row);
896  tmp.resize(A_row, A_row);
897  tmp.mul_tran(Jhat, false);
898  for(i=0; i<A_row; i++)
899  {
900  tmp(i, i) += k;
901  }
902  tmpx.lineq_posv(tmp, wb);
903  mul(tmpx, Jhat);
904  m_info = 0; // ???
905  for(i=0; i<A_col; i++)
906  {
907  (*this)(i) /= w_norm(i);
908  }
909  }
910  else
911  {
912  tmp.resize(A_col, A_col);
913  tmpb.resize(A_col);
914  tmp.mul_tran(Jhat, true);
915  for(i=0; i<A_col; i++)
916  {
917  tmp(i, i) += k;
918  }
919  tmpb.mul(wb, Jhat);
920  lineq_posv(tmp, tmpb);
921  m_info = 0; // ???
922  for(i = 0; i < A_col; i++)
923  {
924  (*this)(i) /= w_norm(i);
925  }
926  }
927  return m_info;
928 }
929 
930 fMat lineq_sr(const fMat& A, fVec& w_err, fVec& w_norm, double k, const fMat& b)
931 {
932  assert(w_err.row() == A.row() && w_norm.row() == A.col() &&
933  b.row() == A.row());
934  fMat x(A.col(), b.col());
935  fMat Jhat, tJhat, wb;
936  Jhat.resize(A.row(), A.col());
937  tJhat.resize(A.col(), A.row());
938  wb.resize(b.row(), b.col());
939  int i, j;
940  Jhat.set(A);
941  wb.set(b);
942  for(i=0; i<A.row(); i++)
943  {
944  if(w_err(i) < 1e-8)
945  {
946  w_err(i) = 1.0;
947  }
948  for(j=0; j<A.col(); j++)
949  {
950  Jhat(i, j) *= w_err(i);
951  }
952  }
953  for(j=0; j<A.col(); j++)
954  {
955  if(w_norm(j) < 1e-8)
956  {
957  w_norm(j) = 1.0;
958  }
959  for(i=0; i<A.row(); i++)
960  {
961  Jhat(i, j) /= w_norm(j);
962  }
963  }
964  tJhat.tran(Jhat);
965  for(i = 0; i < A.row(); i++){
966  for(j = 0; j < b.col(); j++){
967  wb(i, j) *= w_err(i);
968  }
969  }
970  fMat tmp, tmpx, tmpb;
971  if(A.row() < A.col())
972  {
973  tmpx.resize(A.row(), b.col());
974  tmp.resize(A.row(), A.row());
975  tmp.mul(Jhat, tJhat);
976  for(i=0; i<A.row(); i++)
977  {
978  tmp(i, i) += k;
979  }
980  tmpx.lineq(tmp, wb);
981  x.mul(tJhat, tmpx);
982  for(i=0; i<A.col(); i++)
983  {
984  for(j = 0; j < b.col(); j++)
985  {
986  x(i, j) /= w_norm(i);
987  }
988  }
989  }
990  else
991  {
992  tmp.resize(A.col(), A.col());
993  tmpx.resize(A.col(), b.col());
994  tmpb.resize(A.col(), b.col());
995  tmp.mul(tJhat, Jhat);
996  for(i=0; i<A.col(); i++)
997  {
998  tmp(i, i) += k;
999  }
1000  tmpb.mul(tJhat, wb);
1001  x.lineq(tmp, tmpb);
1002  for(i=0; i<A.col(); i++)
1003  {
1004  for(j = 0; j < b.col(); j++)
1005  {
1006  x(i, j) /= w_norm(i);
1007  }
1008  }
1009  }
1010  return x;
1011 }
1012 
1013 fMat tran(const fMat& mat)
1014 {
1015  fMat ret(mat.n_col, mat.n_row);
1016  int i, j, c = mat.n_col, r = mat.n_row;
1017  for(i=0; i<c; i++)
1018  {
1019  for(j=0; j<r; j++)
1020  {
1021  ret(i,j) = mat.p_data[j+i*mat.n_row];
1022  }
1023  }
1024  return ret;
1025 }
1026 
1027 void fMat::tran(const fMat& mat)
1028 {
1029  assert(n_col == mat.n_row && n_row == mat.n_col);
1030  int i, j, n, m, c = mat.n_col, r = mat.n_row;
1031  for(i=0, n=0; i<c; i++, n+=r)
1032  {
1033  for(j=0, m=0; j<r; j++, m+=c)
1034  {
1035  p_data[i+m] = mat.p_data[j+n];
1036  }
1037  }
1038 }
1039 
1041 {
1042  assert(n_col == n_row);
1043  double tmp;
1044  int i, j, m, n, idx1, idx2;
1045  for(i=0, n=0; i<n_row; i++, n+=n_col)
1046  {
1047  for(j=i, m=i*n_row; j<n_col; j++, m+=n_row)
1048  {
1049  idx1 = i + m;
1050  idx2 = j + n;
1051  tmp = p_data[idx1];
1052  p_data[idx1] = p_data[idx2];
1053  p_data[idx2] = tmp;
1054  }
1055  }
1056 }
1057 
1059 {
1060  assert(n_col == n_row);
1061  int i, j, m;
1062  for(i=0; i<n_row; i++)
1063  {
1064  for(j=0, m=i; j<n_col; j++, m+=n_row)
1065  {
1066  if(i==j)
1067  p_data[m] = 1.0;
1068  else
1069  p_data[m] = 0.0;
1070  }
1071  }
1072 }
1073 
1074 void fMat::set(const fMat& mat)
1075 {
1076  assert(n_col == mat.n_col && n_row == mat.n_row);
1077  int n = n_row * n_col;
1078  dims_copy(mat.p_data, p_data, n);
1079 }
1080 
1081 void fMat::neg(const fMat& mat)
1082 {
1083  assert(n_col == mat.n_col && n_row == mat.n_row);
1084  int i, n = n_row * n_col;
1085  for(i=0; i<n; i++) p_data[i] = - mat.p_data[i];
1086 }
1087 
1088 fMat operator - (const fMat& mat)
1089 {
1090  fMat ret(mat.n_row, mat.n_col);
1091  int i, n = mat.n_row * mat.n_col;
1092  for(i=0; i<n; i++) ret.p_data[i] = - mat.p_data[i];
1093  return ret;
1094 }
1095 
1096 void fMat::operator += (const fMat& mat)
1097 {
1098  assert(n_col == mat.n_col && n_row == mat.n_row);
1099  int i, n = n_col * n_row;
1100  for(i=0; i<n; i++) p_data[i] += mat.p_data[i];
1101 }
1102 
1103 void fMat::operator -= (const fMat& mat)
1104 {
1105  assert(n_col == mat.n_col && n_row == mat.n_row);
1106  int i, n = n_col * n_row;
1107  for(i=0; i<n; i++) p_data[i] -= mat.p_data[i];
1108 }
1109 
1110 void fMat::operator *= (double d)
1111 {
1112  int n = n_col * n_row;
1113  dims_scale_myself(p_data, d, n);
1114 }
1115 
1116 void fMat::mul(double d, const fMat& mat)
1117 {
1118  assert(n_col == mat.n_col && n_row == mat.n_row);
1119  int n = n_col * n_row;
1120  dims_scale(mat.data(), d, n, data());
1121 }
1122 
1123 void fMat::mul(const fMat& mat, double d)
1124 {
1125  assert(n_col == mat.n_col && n_row == mat.n_row);
1126  int n = n_col * n_row;
1127  dims_scale(mat.data(), d, n, data());
1128 }
1129 
1130 fMat operator * (double d, const fMat& mat)
1131 {
1132  fMat ret(mat.row(), mat.col());
1133  int i, n = mat.col() * mat.row();
1134  for(i=0; i<n; i++) *(ret.data() + i) = d * *(mat.data() + i);
1135  return ret;
1136 }
1137 
1138 fMat operator * (const fMat& mat, double d)
1139 {
1140  fMat ret(mat.row(), mat.col());
1141  int i, n = mat.col() * mat.row();
1142  for(i=0; i<n; i++) *(ret.data() + i) = d * *(mat.data() + i);
1143  return ret;
1144 }
1145 
1146 void fMat::operator /= (double d)
1147 {
1148  int n = n_col * n_row;
1149  dims_scale_myself(p_data, 1.0/d, n);
1150 }
1151 
1153 {
1154  assert(n_row == mat.n_row && n_col == mat.n_col);
1155  int i, n = n_col * n_row;
1156  for(i=0; i<n; i++)
1157  {
1158  *(p_data + i) = *(mat.data() + i);
1159  }
1160  return *this;
1161 }
1162 
1163 void fMat::operator = (double d)
1164 {
1165  int i, n = n_col * n_row;
1166  for(i=0; i<n; i++) p_data[i] = d;
1167 }
1168 
1169 void fMat::add(const fMat& mat1, const fMat& mat2)
1170 {
1171  assert(n_col == mat1.n_col && n_row == mat1.n_row &&
1172  mat1.n_col == mat2.n_col && mat1.n_row == mat2.n_row);
1173  int i, n = n_row * n_col;
1174  for(i=0; i<n; i++)
1175  {
1176  p_data[i] = mat1.p_data[i] + mat2.p_data[i];
1177  }
1178 }
1179 
1180 void fMat::add(const fMat& mat)
1181 {
1182  assert(n_col == mat.n_col && n_row == mat.n_row);
1183  int i, n = n_row * n_col;
1184  for(i=0; i<n; i++) p_data[i] += mat.p_data[i];
1185 }
1186 
1187 fMat operator + (const fMat& mat1, const fMat& mat2)
1188 {
1189  assert(mat1.n_col == mat2.n_col && mat1.n_row == mat2.n_row);
1190  fMat ret(mat1.n_row, mat1.n_col, 0.0);
1191  int i, n = mat1.n_row * mat1.n_col;
1192  for(i=0; i<n; i++) ret.p_data[i] = mat1.p_data[i] + mat2.p_data[i];
1193  return ret;
1194 }
1195 
1196 void fMat::sub(const fMat& mat1, const fMat& mat2)
1197 {
1198  assert(n_col == mat1.n_col && n_row == mat1.n_row &&
1199  mat1.n_col == mat2.n_col && mat1.n_row == mat2.n_row);
1200  int i, n = n_row * n_col;
1201  double* p1 = mat1.p_data, *p2 = mat2.p_data;
1202  for(i=0; i<n; i++)
1203  {
1204  p_data[i] = p1[i] - p2[i];
1205  }
1206 }
1207 
1208 fMat operator - (const fMat& mat1, const fMat& mat2)
1209 {
1210  assert(mat1.n_col == mat2.n_col && mat1.n_row == mat2.n_row);
1211  fMat ret(mat1.n_row, mat1.n_col, 0.0);
1212  int i, n = mat1.n_row * mat1.n_col;
1213  for(i=0; i<n; i++) ret.p_data[i] = mat1.p_data[i] - mat2.p_data[i];
1214  return ret;
1215 }
1216 
1217 void fMat::mul(const fMat& mat1, const fMat& mat2)
1218 {
1219  assert(n_row == mat1.n_row && n_col == mat2.n_col && mat1.n_col == mat2.n_row);
1220 #if 0
1221  int i, j, k, n, c = mat1.col(), r = mat1.row();
1222  double* p1, *p2, *pp;
1223  for(i=0; i<n_row; i++)
1224  {
1225  for(j=0, n=0, pp=p_data+i; j<n_col; j++, n+=c, pp+=n_row)
1226  {
1227  double temp = 0.0;
1228  for(k=0, p1=mat1.p_data+i, p2=mat2.p_data+n; k<c; k++, p1+=r, p2++)
1229  {
1230  temp += *p1 * *p2;
1231  }
1232  *pp = temp;
1233  }
1234  }
1235 #else
1236  int m = mat1.row(), k = mat1.col(), n = mat2.col();
1237  dims_dgemm(mat1.data(), mat2.data(), m, n, k, p_data);
1238 #endif
1239 }
1240 
1241 void fMat::mul_tran(const fMat& mat1, int trans_first)
1242 {
1243  assert(n_row == n_col);
1244  assert((trans_first && n_row == mat1.n_col) || (!trans_first && n_row ==mat1.n_row));
1245  if(trans_first)
1246  {
1247  int i, j, r = mat1.row();
1248  double* p1 = mat1.p_data, *p2, *pp;
1249  double* pp_first;
1250  for(j=0, p2=mat1.p_data, pp_first=p_data; j<n_col; j++, p2+=r, pp_first+=n_row)
1251  {
1252  pp = pp_first;
1253  for(i=0, p1=mat1.p_data; i<=j; i++, p1+=r, pp++)
1254  {
1255  double temp = dims_dot(p1, p2, r);
1256  *pp = temp;
1257  if(i!=j) p_data[j+n_row*i] = temp;
1258  }
1259  }
1260  }
1261  else
1262  {
1263  dims_dsyrk(mat1.p_data, mat1.n_row, mat1.n_col, p_data);
1264  symmetric();
1265  }
1266 }
1267 
1268 fMat operator * (const fMat& mat1, const fMat& mat2)
1269 {
1270  assert(mat1.n_col == mat2.n_row);
1271  fMat ret(mat1.n_row, mat2.n_col, 0.0);
1272  int i, j, k, n;
1273  for(i=0; i<ret.row(); i++)
1274  {
1275  for(j=0; j<ret.col(); j++)
1276  {
1277  n = j * mat2.n_row;
1278  ret(i, j) = 0;
1279  for(k=0; k<mat1.col(); k++)
1280  {
1281  ret(i, j) += mat1.p_data[i+k*mat1.n_row] * mat2.p_data[k+n];
1282  }
1283  }
1284  }
1285  return ret;
1286 }
1287 
1288 int inv_enlarge(const fMat& m12, const fMat& m21, const fMat& m22, const fMat& P, fMat& X, fMat& y, fMat& z, fMat& w)
1289 {
1290  int n_org = P.col();
1291  int n_add = m12.col();
1292  fMat Pm(n_org, n_add), mPm(n_add, n_add), mP(n_add, n_org);
1293  X.resize(n_org, n_org);
1294  y.resize(n_org, n_add);
1295  z.resize(n_add, n_org);
1296  w.resize(n_add, n_add);
1297  if(n_org == 0)
1298  {
1299  w.inv_svd(m22);
1300  return 0;
1301  }
1302  Pm.mul(P, m12);
1303  mPm.mul(m21, Pm);
1304  mPm *= -1.0;
1305  mPm += m22;
1306  w.inv_svd(mPm);
1307  y.mul(Pm, w);
1308  y *= -1.0;
1309  mP.mul(m21, P);
1310  z.mul(w, mP);
1311  z *= -1.0;
1312  X.mul(Pm, z);
1313  X *= -1.0;
1314  X += P;
1315  return 0;
1316 }
1317 
1318 int inv_shrink(const fMat& P, const fMat& q, const fMat& r, const fMat& s, fMat& X)
1319 {
1320  int n_org = P.col();
1321  int n_add = q.col();
1322  if(n_org == 0)
1323  {
1324  return 0;
1325  }
1326  fMat sr(n_add, n_org), qsr(n_org, n_org);
1327  sr.lineq_svd(s, r);
1328  qsr.mul(q, sr);
1329  X.resize(n_org, n_org);
1330  X.sub(P, qsr);
1331  return 0;
1332 }
1333 
1334 int inv_row_replaced(const fMat& P, const fMat& q, const fMat& m2d, fMat& X, fMat& y)
1335 {
1336  int n_org = P.col();
1337  int n_add = q.col();
1338  int n_total = P.row();
1339  fMat m2d_q(n_add, n_add), m2d_q_inv(n_add, n_add), m2d_P(n_add, n_org);
1340  X.resize(n_total, n_org);
1341  y.resize(n_total, n_add);
1342  if(n_org == 0)
1343  {
1344  y.inv_svd(m2d);
1345  return 0;
1346  }
1347  m2d_q.mul(m2d, q);
1348  m2d_q_inv.inv_svd(m2d_q);
1349  y.mul(q, m2d_q_inv);
1350  m2d_P.mul(m2d, P);
1351  X.mul(y, m2d_P);
1352  X *= -1.0;
1353  X += P;
1354  return 0;
1355 }
1356 
1357 int inv_col_replaced(const fMat& P, const fMat& q, const fMat& m2d, fMat& X, fMat& y)
1358 {
1359  int n_org = P.row();
1360  int n_add = q.row();
1361  int n_total = P.col();
1362  fMat q_m2d(n_add, n_add), q_m2d_inv(n_add, n_add), P_m2d(n_org, n_add);
1363  X.resize(n_org, n_total);
1364  y.resize(n_add, n_total);
1365  if(n_org == 0)
1366  {
1367  y.inv_svd(m2d);
1368  return 0;
1369  }
1370  q_m2d.mul(q, m2d);
1371  q_m2d_inv.inv_svd(q_m2d);
1372  y.mul(q_m2d_inv, q);
1373  P_m2d.mul(P, m2d);
1374  X.mul(P_m2d, y);
1375  X *= -1.0;
1376  X += P;
1377  return 0;
1378 }
1379 
1380 void fVec::mul(const fMat& mat, const fVec& vec)
1381 {
1382  assert(n_row == mat.row() && mat.col() == vec.row());
1383 #if 0
1384  int i, k, c = mat.col(), r = mat.row();
1385  double* pm, *pv, *pp;
1386  for(i=0, pp=p_data; i<n_row; i++, pp++)
1387  {
1388  double temp = 0.0;
1389  for(k=0, pm=mat.data()+i, pv=vec.data(); k<c; k++, pm+=r, pv++)
1390  {
1391  temp += *pm * *pv;
1392  }
1393  *pp = temp;
1394  }
1395 #else
1396  int c = mat.col(), r = mat.row();
1397  dims_dgemv(mat.data(), r, c, vec.data(), p_data);
1398 #endif
1399 }
1400 
1401 void fVec::mul(const fVec& vec, const fMat& mat)
1402 {
1403  assert(n_row == mat.col() && mat.row() == vec.row());
1404  int i, r = mat.row();
1405  double* pm, *pv = vec.p_data, *pp;
1406  for(i=0, pm=mat.data(), pp=p_data; i<n_row; i++, pm+=r, pp++)
1407  {
1408  *pp = dims_dot(pv, pm, r);
1409  }
1410 }
1411 
1412 fVec operator * (const fMat& mat, const fVec& vec)
1413 {
1414  assert(mat.col() == vec.row());
1415  fVec ret(mat.row(), 0.0);
1416  int i, k;
1417  for(i=0; i<ret.row(); i++)
1418  {
1419  ret(i) = 0;
1420  for(k=0; k<mat.col(); k++)
1421  {
1422  ret(i) += mat.data()[i+k*mat.row()]*vec.data()[k];
1423  }
1424  }
1425  return ret;
1426 }
1427 
1428 void fMat::div(const fMat& mat, double d)
1429 {
1430  assert(n_col == mat.n_col && n_row == mat.n_row);
1431  int n = n_col * n_row;
1432  dims_scale(mat.p_data, 1.0/d, n, p_data);
1433 }
1434 
1435 fMat operator / (const fMat& mat, double d)
1436 {
1437  fMat ret(mat.n_row, mat.n_col);
1438  int i, n = mat.col() * mat.row();
1439  for(i=0; i<n; i++) ret.p_data[i] = mat.p_data[i] / d;
1440  return ret;
1441 }
1442 
1447 {
1448  int ret = -1;
1449  double max_val = 0.0;
1450  int i;
1451  for(i=0; i<n_row; i++)
1452  {
1453  if(ret < 0 || p_data[i] > max_val)
1454  {
1455  ret = i;
1456  max_val = p_data[i];
1457  }
1458  }
1459  return ret;
1460 }
1461 
1463 {
1464  int ret = -1;
1465  double min_val = 0.0;
1466  int i;
1467  for(i=0; i<n_row; i++)
1468  {
1469  if(ret < 0 || p_data[i] < min_val)
1470  {
1471  ret = i;
1472  min_val = p_data[i];
1473  }
1474  }
1475  return ret;
1476 }
1477 
1479 {
1480  int index = -1;
1481  double max_val = 0.0;
1482  int i;
1483  for(i=0; i<n_row; i++)
1484  {
1485  if(index < 0 || p_data[i] > max_val)
1486  {
1487  index = i;
1488  max_val = p_data[i];
1489  }
1490  }
1491  return max_val;
1492 }
1493 
1495 {
1496  int index = -1;
1497  double min_val = 0.0;
1498  int i;
1499  for(i=0; i<n_row; i++)
1500  {
1501  if(index < 0 || p_data[i] < min_val)
1502  {
1503  index = i;
1504  min_val = p_data[i];
1505  }
1506  }
1507  return min_val;
1508 }
1509 
1511 {
1512  for(int i=0; i<n_row; i++)
1513  {
1514  p_data[i] = fabs(p_data[i]);
1515  }
1516 }
1517 
1518 void fVec::set(const fVec& vec)
1519 {
1520  assert(n_row == vec.n_row);
1521  dims_copy(vec.p_data, p_data, n_row);
1522 }
1523 
1524 void fVec::neg(const fVec& vec)
1525 {
1526  assert(n_row == vec.n_row);
1527  int i;
1528  for(i=0; i<vec.n_row; i++) p_data[i] = - vec.p_data[i];
1529 }
1530 
1531 fVec operator - (const fVec& vec)
1532 {
1533  fVec ret(vec.n_row);
1534  int i;
1535  for(i=0; i<vec.n_row; i++) ret.p_data[i] = - vec.p_data[i];
1536  return ret;
1537 }
1538 
1539 void fVec::operator += (const fVec& vec)
1540 {
1541  assert(n_row == vec.n_row);
1542  int i;
1543  for(i=0; i<n_row; i++) p_data[i] += vec.p_data[i];
1544 }
1545 
1546 void fVec::operator -= (const fVec& vec)
1547 {
1548  assert(n_row == vec.n_row);
1549  dims_daxpy(n_row, -1.0, vec.p_data, p_data);
1550 }
1551 
1552 void fVec::operator *= (double d)
1553 {
1555 }
1556 
1557 void fVec::operator /= (double d)
1558 {
1559  dims_scale_myself(p_data, 1.0/d, n_row);
1560 }
1561 
1562 void fVec::add(const fVec& vec1, const fVec& vec2)
1563 {
1564  assert(n_row == vec1.n_row && vec1.n_row == vec2.n_row);
1565  for(int i=0; i<n_row; i++) p_data[i] = vec1.p_data[i] + vec2.p_data[i];
1566 }
1567 
1568 void fVec::add(const fVec& vec)
1569 {
1570  assert(n_row == vec.n_row);
1571  for(int i=0; i<n_row; i++) p_data[i] += vec.p_data[i];
1572 }
1573 
1574 fVec operator + (const fVec& vec1, const fVec& vec2)
1575 {
1576  assert(vec1.n_row == vec2.n_row);
1577  fVec ret(vec1.row(), 0.0);
1578  for(int i=0; i<vec1.n_row; i++)
1579  ret.p_data[i] = vec1.p_data[i] + vec2.p_data[i];
1580  return ret;
1581 }
1582 
1583 void fVec::sub(const fVec& vec1, const fVec& vec2)
1584 {
1585  assert(n_row == vec1.n_row && vec1.n_row == vec2.n_row);
1586  for(int i=0; i<n_row; i++)
1587  p_data[i] = vec1.p_data[i] - vec2.p_data[i];
1588 }
1589 
1590 fVec operator - (const fVec& vec1, const fVec& vec2)
1591 {
1592  assert(vec1.n_row == vec2.n_row);
1593  fVec ret(vec1.row(), 0.0);
1594  for(int i=0; i<vec1.n_row; i++)
1595  ret.p_data[i] = vec1.p_data[i] - vec2.p_data[i];
1596  return ret;
1597 }
1598 
1599 void fVec::div(const fVec& vec, double d)
1600 {
1601  assert(n_row == vec.n_row);
1602  dims_scale(vec.p_data, 1.0/d, n_row, p_data);
1603 }
1604 
1605 fVec operator / (const fVec& vec, double d)
1606 {
1607  fVec ret(vec.n_row);
1608  dims_scale(vec.p_data, 1.0/d, vec.n_row, ret.p_data);
1609  return ret;
1610 }
1611 
1612 double operator * (const fVec& vec1, const fVec& vec2)
1613 {
1614  assert(vec1.n_row == vec2.n_row);
1615  double ret = 0;
1616  ret = dims_dot(vec1.p_data, vec2.p_data, vec1.n_row);
1617  return ret;
1618 }
1619 
1620 void fVec::mul(const fVec& vec, double d)
1621 {
1622  assert(n_row == vec.n_row);
1623  dims_scale(vec.p_data, d, n_row, p_data);
1624 }
1625 
1626 void fVec::mul(double d, const fVec& vec)
1627 {
1628  assert(n_row == vec.n_row);
1629  dims_scale(vec.p_data, d, n_row, p_data);
1630 }
1631 
1632 fVec operator * (double d, const fVec& vec)
1633 {
1634  fVec ret(vec.n_row);
1635  dims_scale(vec.p_data, d, vec.n_row, ret.p_data);
1636  return ret;
1637 }
1638 
1639 fVec operator * (const fVec& vec, double d)
1640 {
1641  fVec ret(vec.row());
1642  dims_scale(vec.p_data, d, vec.n_row, ret.p_data);
1643  return ret;
1644 }
1645 
1646 void fVec::operator = (double d)
1647 {
1648  for(int i=0; i<n_row; i++) p_data[i] = d;
1649 }
1650 
1652 {
1653  assert(n_row == vec.n_row);
1654  dims_copy(vec.p_data, p_data, n_row);
1655  return *this;
1656 }
1657 
1658 /**********added by takano*******/
1659 fMat eigs(const fMat& mat,double* w)
1660 {
1661  int i,j,k,dim;
1662  double *a;
1663  fMat eigmat;
1664  a= new double [mat.col()*mat.row()];
1665 // w=(double*)malloc(sizeof(double)*mat.col());
1666 
1667  k=0;
1668  for(i=0;i<mat.row();i++){
1669  for(j=0;j<mat.col();j++){
1670  a[k]=mat(i,j);
1671  k=k+1;
1672  }
1673  }
1674  dim=mat.row();
1675  i=dims_eigs(dim,a,w);
1676  eigmat.resize(dim,dim);
1677  k=0;
1678  for(j=0;j<dim;j++){
1679  for(i=0;i<dim;i++){
1680  eigmat(i,j)=a[k];
1681  k=k+1;
1682  }
1683  }
1684  delete [] a;
1685  return eigmat;
1686 }
1687 
1688 void fMat::dmul(const fMat& mat1, const fMat& mat2)
1689 {
1690 #ifndef NDEBUG
1691 // if(n_row != mat1.n_row || n_col != mat2.n_col ||
1692 // mat1.n_col != mat2.n_row)
1693 // {
1694 // cerr << "fMat::mul(fMat, fMat): matrix size error" << endl;
1695 // return;
1696 // }
1697 #endif
1698  int i, j, m;
1699  if(mat1.n_row == mat2.n_row && mat1.n_col == mat2.n_col)
1700  {
1701  for(i=0; i<n_row; i++)
1702  {
1703  for(j=0, m=i; j<n_col; j++, m+=n_row)
1704  {
1705  p_data[m] = mat1.p_data[m] * mat2.p_data[m];
1706  }
1707  }
1708  }
1709  else if(mat1.n_row == mat2.n_row && mat2.n_col == 1)
1710  {
1711  for(i=0; i<n_row; i++)
1712  {
1713  for(j=0, m=i; j<n_col; j++, m+=n_row)
1714  {
1715  p_data[m] = mat1.p_data[m] * mat2.p_data[i];
1716  }
1717  }
1718 
1719  }
1720  else if(mat1.n_col == mat2.n_col && mat2.n_row == 1)
1721  {
1722  for(i=0; i<n_row; i++)
1723  {
1724  for(j=0, m=i; j<n_col; j++, m+=n_row)
1725  {
1726  p_data[m] = mat1.p_data[m] * mat2.p_data[j];
1727  }
1728  }
1729  }
1730  else
1731  {
1732  cerr << "fMat::dmul(fMat, fMat): matrix size error" << endl;
1733  }
1734 }
1735 
1736 
1737 void fMat::ddiv(const fMat& mat1, const fMat& mat2)
1738 {
1739 #ifndef NDEBUG
1740 // if(n_row != mat1.n_row || n_col != mat2.n_col ||
1741 // mat1.n_col != mat2.n_row)
1742 // {
1743 // cerr << "fMat::mul(fMat, fMat): matrix size error" << endl;
1744 // return;
1745 // }
1746 #endif
1747  int i, j, m;
1748  if(mat1.n_row == mat2.n_row && mat1.n_col == mat2.n_col)
1749  {
1750  for(i=0; i<n_row; i++)
1751  {
1752  for(j=0, m=i; j<n_col; j++, m+=n_row)
1753  {
1754  p_data[m] = mat1.p_data[m] / mat2.p_data[m];
1755  }
1756  }
1757  }
1758  else if(mat1.n_row == mat2.n_row && mat2.n_col == 1)
1759  {
1760  for(i=0; i<n_row; i++)
1761  {
1762  for(j=0, m=i; j<n_col; j++, m+=n_row)
1763  {
1764  p_data[m] = mat1.p_data[m] / mat2.p_data[i];
1765  }
1766  }
1767 
1768  }
1769  else if(mat1.n_col == mat2.n_col && mat2.n_row == 1)
1770  {
1771  for(i=0; i<n_row; i++)
1772  {
1773  for(j=0, m=i; j<n_col; j++, m+=n_row)
1774  {
1775  p_data[m] = mat1.p_data[m] / mat2.p_data[j];
1776  }
1777  }
1778  }
1779  else
1780  {
1781  cerr << "fMat::dmul(fMat, fMat): matrix size error" << endl;
1782  }
1783 }
1784 
1785 double fMat::det(void)
1786 {
1787 #ifndef NDEBUG
1788  if((n_row !=n_col) || (n_row < 1))
1789  {
1790  cerr << "matrix size error at function det()" << endl;
1791  return 0;
1792  }
1793 #endif
1794  double x;
1795  m_info = dims_det(n_row, p_data, &x);
1796 
1797  return x;
1798 }
1799 
1800 int fMat::eig(fVec& wr, fVec& wi)
1801 {
1802 #ifndef NDEBUG
1803  if((n_row !=n_col) || (n_row < 1) ||
1804  (n_row !=wr.row()) || (n_row !=wi.row()))
1805  {
1806  cerr << "matrix size error at function eig()" << endl;
1807  return -1;
1808  }
1809 #endif
1810 
1812  p_data,
1813  wr.data(),
1814  wi.data());
1815 
1816  return m_info;
1817 }
1818 
1819 int fMat::eig(fVec& wr, fVec& wi, fMat& v)
1820 {
1821 #ifndef NDEBUG
1822  if((n_row !=n_col) || (n_row < 1) ||
1823  (n_row !=wr.row()) || (n_row !=wi.row()) ||
1824  (n_row !=v.row()) || (n_row !=v.col()))
1825  {
1826  cerr << "matrix size error at function eig()" << endl;
1827  return -1;
1828  }
1829 #endif
1830 
1831  m_info = dims_dgeev(n_row,
1832  p_data,
1833  wr.data(),
1834  wi.data(),
1835  v.data());
1836 
1837  return m_info;
1838 }
1839 
1840 int fMat::eig(fVec& wr, fVec& wi, fMat& vr, fMat& vi)
1841 {
1842 #ifndef NDEBUG
1843  if((n_row !=n_col) || (n_row < 1) ||
1844  (n_row !=wr.row()) || (n_row !=wi.row()) ||
1845  (n_row !=vr.row()) || (n_row !=vr.col()) ||
1846  (n_row !=vi.row()) || (n_row !=vi.col()) )
1847  {
1848  cerr << "matrix size error at function eig()" << endl;
1849  return -1;
1850  }
1851 #endif
1852  int i, j;
1853  double *p_wi, *p_vr, *p_vi;
1854 
1855  p_wi = wi.data();
1856  p_vr = vr.data();
1857  p_vi = vi.data();
1858 
1859  m_info = dims_dgeev(n_row,
1860  p_data,
1861  wr.data(),
1862  p_wi,
1863  p_vr);
1864 
1865  for(i=0;i<n_row;i++, p_wi++, p_vr+=n_row, p_vi+=n_row)
1866  if(*p_wi == 0)
1867  for(j=0;j<n_row;j++)
1868  *(p_vi+j) = 0.0;
1869  else
1870  {
1871  p_vr+=n_row;
1872  for(j=0;j<n_row;j++)
1873  *(p_vi+j) = *(p_vr+j);
1874 
1875  p_vi+=n_row;
1876  for(j=0;j<n_row;j++)
1877  *(p_vi+j) = -*(p_vr+j);
1878 
1879  for(j=0;j<n_row;j++)
1880  *(p_vr+j) = *(p_vr+j-n_row);
1881 
1882  i++, p_wi++;
1883  }
1884 
1885  return m_info;
1886 }
friend fMat p_inv(const fMat &mat)
pseudo inverse
Definition: fMatrix.cpp:191
int dims_dgeev_simple(int _n, double *_a, double *_wr, double *_wi)
Computes eigenvalues only.
int c
Definition: autoplay.py:16
int lineq_svd(const fMat &A, const fVec &b, int lwork=-1)
Definition: fMatrix.cpp:468
void ddiv(const fMat &mat1, const fMat &mat2)
Element-wise division: ./ operator in MATLAB.
Definition: fMatrix.cpp:1737
void sub(const fMat &mat1, const fMat &mat2)
Definition: fMatrix.cpp:1196
int dims_dgemv(double *_A, int _m, int _n, double *_x, double *_y)
void set_subvec(int start, const fVec &subV)
Copy a smaller vector as a sub-vector.
Definition: fMatrix.cpp:76
int lineq_porfs(const fMat &A, const fMat &b)
solve linear equation Ax = b, where A is positive-definite, symmetric
Definition: fMatrix.cpp:772
int resize(int i, int j)
Changes the matrix size.
Definition: fMatrix.h:133
void operator=(double d)
Assignment operations.
Definition: fMatrix.cpp:1646
* x
Definition: IceUtils.h:98
friend fMat operator*(const fMat &mat1, const fMat &mat2)
Definition: fMatrix.cpp:1268
int eig(fVec &wr, fVec &wi)
Computes the eigenvalues.
Definition: fMatrix.cpp:1800
friend int inv_col_replaced(const fMat &P, const fMat &q, const fMat &m2d, fMat &X, fMat &y)
Computes the inverse when some columns are replaced.
Definition: fMatrix.cpp:1357
fMat()
Default constructor.
Definition: fMatrix.h:53
friend fMat inv(const fMat &mat)
inverse
Definition: fMatrix.cpp:176
int lineq_porfs(const fMat &A, const fVec &b)
Definition: fMatrix.cpp:788
int dims_scale(double *_x, double _alpha, int _n, double *_y)
void add(const fMat &mat1, const fMat &mat2)
Definition: fMatrix.cpp:1169
int col() const
Returns the number of columns.
Definition: fMatrix.h:154
double min_value()
Returns the minimum value.
Definition: fMatrix.cpp:1494
friend int inv_shrink(const fMat &P, const fMat &q, const fMat &r, const fMat &s, fMat &X)
Computes the inverse of a shrinked matrix.
Definition: fMatrix.cpp:1318
Definition: clap.cpp:13
int row() const
Returns the number of rows.
Definition: fMatrix.h:158
double dims_dot(double *_x, double *_y, int _n)
int dims_det(int _n, double *_a, double *_x)
Computes the determinant.
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
void operator-=(const fMat &mat)
Definition: fMatrix.cpp:1103
int inv_porfs(const fMat &)
inverse of positive-definite, symmetric matrix
Definition: fMatrix.cpp:780
#define MIN(a, b)
Returns the min value between a and b.
Definition: IceTypes.h:146
int max_index()
Returns the index of the largest element.
Definition: fMatrix.cpp:1446
int dims_dgeev(int _n, double *_a, double *_wr, double *_wi, double *_vr)
Computes eigenvalues and eigenvectors.
int m_info
Definition: fMatrix.h:482
int dims_eigs(int _n, double *_a, double *w)
Eigenvalues / eigenvectors.
png_uint_32 i
Definition: png.h:2735
void mul_tran(const fMat &mat1, int trans_first)
Definition: fMatrix.cpp:1241
int lineq_posv(const fMat &A, const fMat &b)
solve linear equation Ax = b, where A is positive-definite, symmetric
Definition: fMatrix.cpp:746
long b
Definition: jpegint.h:371
friend fMat lineq_sr(const fMat &A, fVec &w_err, fVec &w_norm, double k, const fMat &b)
solve linear equation Ax = b using SR-inverse
Definition: fMatrix.cpp:930
void operator-=(const fVec &vec)
Definition: fMatrix.cpp:1546
friend int inv_row_replaced(const fMat &P, const fMat &q, const fMat &m2d, fMat &X, fMat &y)
Computes the inverse when some rows are replaced.
Definition: fMatrix.cpp:1334
void mul(const fMat &mat1, const fMat &mat2)
Definition: fMatrix.cpp:1217
void operator+=(const fVec &vec)
Definition: fMatrix.cpp:1539
Generic matrix/vector classes.
double * p_data
Definition: fMatrix.h:480
void operator*=(double d)
Definition: fMatrix.cpp:1552
double det(void)
Computes the determinant.
Definition: fMatrix.cpp:1785
friend fMat lineq(const fMat &A, const fMat &b)
solve linear equation Ax = b
Definition: fMatrix.cpp:137
void set(double *_d)
Sets all elements.
Definition: fMatrix.h:533
void mul(const fVec &vec, double d)
Definition: fMatrix.cpp:1620
void operator/=(double d)
Definition: fMatrix.cpp:1146
double temp
Definition: fMatrix.h:481
def j(str, encoding="cp932")
friend fMat operator/(const fMat &mat, double d)
Definition: fMatrix.cpp:1435
list index
void sub(const fVec &vec1, const fVec &vec2)
Definition: fMatrix.cpp:1583
friend fMat lineq_svd(const fMat &A, const fMat &b, int lwork)
solve linear equation Ax = b
Definition: fMatrix.cpp:416
void div(const fVec &vec, double d)
Definition: fMatrix.cpp:1599
void tran()
Transposes a matrix (only for square matrices).
Definition: fMatrix.cpp:1040
void abs()
Converts all elements to their absolute values.
Definition: fMatrix.cpp:1510
void get_submat(int row_start, int col_start, const fMat &allM)
Extract a sub-matrix and set to myself.
Definition: fMatrix.cpp:32
string a
void get_subvec(int start, const fVec &allV)
Copy a sub-vector of a larger vector.
Definition: fMatrix.cpp:65
int dims_daxpy(int _n, double _alpha, double *_x, double *_y)
void operator*=(double d)
Definition: fMatrix.cpp:1110
int lineq_posv(const fMat &A, const fVec &b)
Definition: fMatrix.cpp:765
int svd(fMat &U, fVec &Sigma, fMat &VT)
singular value decomposition
Definition: fMatrix.cpp:733
void neg(const fMat &mat)
Definition: fMatrix.cpp:1081
void resize(int i)
Change the size.
Definition: fMatrix.h:511
double * data() const
Returns the pointer to the first element.
Definition: fMatrix.h:193
Vector of generic size.
Definition: fMatrix.h:491
friend fMat operator+(const fMat &mat1, const fMat &mat2)
Definition: fMatrix.cpp:1187
friend fMat eigs(const fMat &mat, double *w)
Compute the eigenvalues.
Definition: fMatrix.cpp:1659
png_size_t start
Definition: png.h:1496
int dims_dporfs(double *_a, double *_x, double *_b, int _m, int _nrhs)
For positive-definite, symmetric matrices.
int dims_dgesvx(double *_a, double *_x, double *_b, int _n, int _nrhs)
Solves linear equation using LU decomposition.
int min_index()
Returns the index of the smallest element.
Definition: fMatrix.cpp:1462
int dims_dgemm(double *_A, double *_B, int _m, int _n, int _k, double *_C)
void div(const fMat &mat, double d)
Definition: fMatrix.cpp:1428
void set_submat(int row_start, int col_start, const fMat &subM)
Sets a sub-matrix of myself.
Definition: fMatrix.cpp:48
void operator+=(const fMat &mat)
Definition: fMatrix.cpp:1096
friend fMat inv_svd(const fMat &mat, int lwork)
inverse
Definition: fMatrix.cpp:484
friend fMat tran(const fMat &mat)
Returns the transpose of a matrix.
Definition: fMatrix.cpp:1013
void set(double *_d)
Sets the values from an array.
Definition: fMatrix.h:187
int n_col
Definition: fMatrix.h:483
int n_row
Definition: fMatrix.h:484
void symmetric(char t= 'U')
Change to symmetric matrix.
Definition: fMatrix.cpp:88
friend fMat sr_inv_svd(const fMat &mat, fVec &w_err, fVec &w_norm, double k, int lwork)
SR-inverse.
Definition: fMatrix.cpp:536
friend fMat p_inv_svd(const fMat &mat, int lwork)
pseudo inverse
Definition: fMatrix.cpp:501
int dims_scale_myself(double *_x, double _alpha, int _n)
double max_value()
Returns the maximum value.
Definition: fMatrix.cpp:1478
int inv_posv(const fMat &)
inverse of positive-definite, symmetric matrix
Definition: fMatrix.cpp:757
int dims_dposv(double *_a, double *_x, double *_b, int _m, int _nrhs)
int size() const
Size of the vector (same as row()).
Definition: fMatrix.h:507
* y
Definition: IceUtils.h:97
fMat operator=(const fMat &mat)
Assignment from a reference matrix.
Definition: fMatrix.cpp:1152
int dims_dsyrk(double *_A, int _n, int _k, double *_C)
friend int inv_enlarge(const fMat &m12, const fMat &m21, const fMat &m22, const fMat &P, fMat &X, fMat &y, fMat &z, fMat &w)
Computes the inverse of an enlarged matrix.
Definition: fMatrix.cpp:1288
void dmul(const fMat &mat1, const fMat &mat2)
Element-wise multiplication: .* operator in MATLAB.
Definition: fMatrix.cpp:1688
void identity()
Creates an identity matrix (only for square matrices).
Definition: fMatrix.cpp:1058
void add(const fVec &vec1, const fVec &vec2)
Definition: fMatrix.cpp:1562
int dims_copy(double *_x, double *_y, int _n)
Wrappers of BLAS functions.
void neg(const fVec &vec)
Definition: fMatrix.cpp:1524
void operator/=(double d)
Definition: fMatrix.cpp:1557
int lineq(const fMat &A, const fVec &b)
Solves linear equation Ax = b.
Definition: fMatrix.cpp:163
friend fMat sr_inv(const fMat &mat, fVec &w_err, fVec &w_norm, double k)
singularity-robust (SR) inverse
Definition: fMatrix.cpp:226
int dims_dgelss(double *_a, double *_x, double *_b, int _m, int _n, int _nrhs, double *_s, int *_rank, int _lwork)
Solves linear equation using singular-value decomposition.
Matrix of generic size. The elements are stored in a one-dimensional array in row-major order...
Definition: fMatrix.h:46
int lineq_sr(const fMat &A, fVec &w_err, fVec &w_norm, double k, const fVec &b)
Definition: fMatrix.cpp:869
int dims_svd(double *_a, int _m, int _n, double *_u, double *_sigma, double *_vt)
Performs singular value decomposition.
friend fMat operator-(const fMat &mat)
Definition: fMatrix.cpp:1088


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat Apr 13 2019 02:14:21