fMatrix.h
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 #ifndef __F_MATRIX_H__
17 #define __F_MATRIX_H__
18 
19 #include <dims_common.h>
20 #include <dims_clapack.h>
21 #if defined(__ia64) || defined(__x68_64)
22 #include <cmath>
23 #else
24 #include <math.h>
25 #endif
26 
27 #include <iostream>
28 using namespace std;
29 
30 #include <assert.h>
31 
32 class fVec;
33 class fMat;
34 
35 fMat inv_svd(const fMat& mat, int lwork = -1);
36 fMat p_inv_svd(const fMat& mat, int lwork = -1);
37 fMat sr_inv_svd(const fMat& mat, fVec& w_err, fVec& w_norm, double k, int lwork = -1);
38 fMat lineq_svd(const fMat& A, const fMat& b, int lwork = -1);
39 
40 
46 class fMat
47 {
48 public:
50 
53  fMat() {
54  p_data = 0;
55  n_col = 0;
56  n_row = 0;
57  m_info = 0;
58  temp = 0.0;
59  }
61 
67  fMat(int m, int n, double* ini = 0) {
68  p_data = 0;
69  n_col = 0;
70  n_row = 0;
71  m_info = 0;
72  temp = 0.0;
73  resize(m, n);
74  if(ini)
75  {
76  int k = m * n;
77  for(int i=0; i<k; i++) p_data[i] = ini[i];
78  }
79  }
81 
87  fMat(int m, int n, double ini) {
88  p_data = 0;
89  n_col = 0;
90  n_row = 0;
91  m_info = 0;
92  temp = 0.0;
93  resize(m, n);
94  int k = m * n;
95  for(int i=0; i<k; i++) p_data[i] = ini;
96  }
98 
102  fMat(const fMat& ini) {
103  p_data = 0;
104  n_col = 0;
105  n_row = 0;
106  m_info = ini.m_info;
107  temp = ini.temp;
108  resize(ini.row(), ini.col());
109  int k = ini.row() * ini.col();
110  for(int i=0; i<k; i++) p_data[i] = ini.data()[i];
111  }
113 
117  fMat(const fVec& ini);
118 
120  ~fMat() {
121  if(p_data) delete[] p_data;
122  }
123 
125 
133  int resize(int i, int j) {
134  if(n_row == i && n_col == j) return 0;
135  if(p_data) delete[] p_data;
136  p_data = 0;
137  n_row = i;
138  n_col = j;
139  if(n_row > 0 && n_col > 0)
140  {
141  int nn = n_row * n_col;
142  p_data = new double[nn];
143  if(!p_data)
144  {
145  n_row = 0;
146  n_col = 0;
147  m_info = 1;
148  return -1;
149  }
150  }
151  return 0;
152  }
154  int col() const {
155  return n_col;
156  }
158  int row() const {
159  return n_row;
160  }
162  int info() {
163  return m_info;
164  }
166  void setinfo(int _info) {
167  m_info = _info;
168  }
169 
171  double& operator() (int i, int j) {
172  assert(i>=0 && i<n_row && j>=0 && j<n_col);
173  return *(p_data + i + j*n_row);
174  }
175 
177  double operator() (int i, int j) const {
178  assert(i>=0 && i<n_row && j>=0 && j<n_col);
179  return *(p_data + i + j*n_row);
180  }
181 
183  fMat operator = (const fMat& mat);
185  void operator = (double d);
187  void set(double* _d) {
188  int n_elements = n_row * n_col;
189  dims_copy(_d, p_data, n_elements);
190  }
191 
193  double* data() const {
194  return p_data;
195  }
197  operator double *() {
198  return p_data;
199  }
200 
202  friend ostream& operator << (ostream& ost, const fMat& mat) {
203  int i, j;
204  ost << "[" << mat.row() << ", " << mat.col() << "]" << endl;
205  for(i=0; i<mat.row(); i++)
206  {
207  for(j=0; j<mat.col(); j++)
208  {
209  ost << *(mat.data() + i + j*mat.row());
210  if(j != mat.col()-1) ost << "\t";
211  }
212  if(i == mat.row()-1) ost << flush;
213  else ost << endl;
214  }
215  return ost;
216  }
217 
219 
225  void set_submat(int row_start, int col_start, const fMat& subM);
226 
228 
234  void get_submat(int row_start, int col_start, const fMat& allM);
235 
237  friend fMat tran(const fMat& mat);
238 
240  void tran(const fMat& mat);
241 
243  void tran();
244 
246  void identity();
247 
249  void zero() {
250  int ndata = n_row * n_col;
251  for(int i=0; i<ndata; i++) p_data[i] = 0.0;
252  }
253 
264  friend fMat inv(const fMat& mat);
267  friend fMat p_inv(const fMat& mat);
269  friend fMat sr_inv(const fMat& mat, fVec& w_err, fVec& w_norm, double k);
271  friend fMat lineq(const fMat& A, const fMat& b);
273  friend fMat lineq_sr(const fMat& A, fVec& w_err, fVec& w_norm, double k, const fMat& b);
280  int inv(const fMat&);
283  int p_inv(const fMat&);
285  int sr_inv(const fMat& mat, fVec& w_err, fVec& w_norm, double k);
287  int lineq(const fMat& A, const fMat& b);
289  int lineq_sr(const fMat& A, fVec& w_err, fVec& w_norm, double k, const fMat& b);
292 
303  friend fMat inv_svd(const fMat& mat, int lwork);
306  friend fMat p_inv_svd(const fMat& mat, int lwork);
308  friend fMat sr_inv_svd(const fMat& mat, fVec& w_err, fVec& w_norm, double k, int lwork);
310  friend fMat lineq_svd(const fMat& A, const fMat& b, int lwork);
317  int inv_svd(const fMat&, int lwork = -1);
320  int p_inv_svd(const fMat&, int lwork = -1);
322  int sr_inv_svd(const fMat& mat, fVec& w_err, fVec& w_norm, double k, int lwork = -1);
324  int lineq_svd(const fMat& A, const fMat& b, int lwork = -1);
327 
329  int inv_posv(const fMat&);
331  int lineq_posv(const fMat& A, const fMat& b);
333  int inv_porfs(const fMat&);
335  int lineq_porfs(const fMat& A, const fMat& b);
336 
342  friend fMat operator - (const fMat& mat);
343  void operator += (const fMat& mat);
344  void operator -= (const fMat& mat);
345  void operator *= (double d);
346  void operator /= (double d);
347 
348  friend fMat operator + (const fMat& mat1, const fMat& mat2);
349  friend fMat operator - (const fMat& mat1, const fMat& mat2);
350  friend fMat operator * (const fMat& mat1, const fMat& mat2);
351  friend fVec operator * (const fMat& mat, const fVec& vec);
352  friend fMat operator * (double d, const fMat& mat);
353  friend fMat operator * (const fMat& mat, double d);
354  friend fMat operator / (const fMat& mat, double d);
362  void set(const fMat& mat);
363  void neg(const fMat& mat);
364  void add(const fMat& mat1, const fMat& mat2);
365  void add(const fMat& mat);
366  void sub(const fMat& mat1, const fMat& mat2);
367  void mul(const fMat& mat1, const fMat& mat2);
368  void mul(double d, const fMat& mat);
369  void mul(const fMat& mat, double d);
370  void mul_tran(const fMat& mat1, int trans_first);
371  void div(const fMat& mat, double d);
374  void dmul(const fMat& mat1, const fMat& mat2);
377  void ddiv(const fMat& mat1, const fMat& mat2);
378 
380 
386  int svd(fMat& U, fVec& Sigma, fMat& VT);
387 
389 
393  void symmetric(char t = 'U');
394 
396  double det(void);
397 
399  friend fMat eigs(const fMat& mat, double *w);
401  friend fMat eigs2(const fMat& mat, double *w);
402 
404 
409  int eig(fVec& wr, fVec& wi);
410 
412 
418  int eig(fVec& wr, fVec& wi, fMat& v);
419 
421 
428  int eig(fVec& wr, fVec& wi, fMat& vr, fMat& vi);
429 
431 
438  friend int inv_enlarge(const fMat& m12, const fMat& m21, const fMat& m22, const fMat& P, fMat& X, fMat& y, fMat& z, fMat& w);
439 
441 
449  friend int inv_shrink(const fMat& P, const fMat& q, const fMat& r, const fMat& s, fMat& X);
450 
452 
463  friend int inv_row_replaced(const fMat& P, const fMat& q, const fMat& m2d, fMat& X, fMat& y);
464 
466 
477  friend int inv_col_replaced(const fMat& P, const fMat& q, const fMat& m2d, fMat& X, fMat& y);
478 
479 protected:
480  double* p_data;
481  double temp;
482  int m_info;
483  int n_col;
484  int n_row;
485 };
486 
491 class fVec
492  : public fMat
493 {
494 public:
495  fVec() : fMat() {
496  }
497  fVec(int m, double* ini = 0) : fMat(m, 1, ini) {
498  }
499  fVec(int m, double ini) : fMat(m, 1, ini) {
500  }
501  fVec(const fVec& ini) : fMat(ini) {
502  }
503  ~fVec() {
504  }
505 
507  int size() const {
508  return n_row;
509  }
511  void resize(int i) {
512  if(n_row == i) return;
513  if(p_data) delete[] p_data;
514  p_data = 0;
515  n_row = i;
516  n_col = 1;
517  if(n_row > 0)
518  {
519  p_data = new double[n_row];
520  }
521  }
522 
524  void get_subvec(int start, const fVec& allV);
526  void set_subvec(int start, const fVec& subV);
527 
529  void operator = (double d);
530  fVec operator = (const fVec& vec);
531 
533  void set(double* _d) {
534  dims_copy(_d, p_data, n_row);
535  }
536 
538  double& operator () (int i) {
539  assert(i >= 0 && i < n_row);
540  return *(p_data + i);
541  }
543  double operator () (int i) const {
544  assert(i >= 0 && i < n_row);
545  return *(p_data + i);
546  }
547 
549  friend double length(const fVec& v) {
550  return sqrt(v*v);
551  }
553  double length() const {
554  return sqrt((*this) * (*this));
555  }
557  friend fVec unit(const fVec& v) {
558  fVec ret(v.n_col);
559  double len = v.length();
560  ret = v / len;
561  return ret;
562  }
564  void unit() {
565  double len = length();
566  (*this) /= len;
567  }
569  double sum() {
570  double ret = 0.0;
571  for(int i=0; i<n_row; i++) ret += p_data[i];
572  return ret;
573  }
575  void zero() {
576  for(int i=0; i<n_row; i++) p_data[i] = 0;
577  }
578 
580  int max_index();
582  double max_value();
584  int min_index();
586  double min_value();
587 
589  void abs();
590 
592  int lineq(const fMat& A, const fVec& b);
593  int lineq_svd(const fMat& A, const fVec& b, int lwork = -1);
594  int lineq_posv(const fMat& A, const fVec& b);
595  int lineq_porfs(const fMat& A, const fVec& b);
596  int lineq_sr(const fMat& A, fVec& w_err, fVec& w_norm, double k, const fVec& b);
597 
603  friend fVec operator - (const fVec& vec);
604  void operator += (const fVec& vec);
605  void operator -= (const fVec& vec);
606  void operator *= (double d);
607  void operator /= (double d);
608 
609  friend fVec operator + (const fVec& vec1, const fVec& vec2);
610  friend fVec operator - (const fVec& vec1, const fVec& vec2);
611  friend fVec operator / (const fVec& vec, double d);
612  friend fVec operator * (double d, const fVec& vec);
613  friend fVec operator * (const fVec& vec, double d);
614  friend double operator * (const fVec& vec1, const fVec& vec2);
622  void set(const fVec& vec);
623  void neg(const fVec& vec);
624  void add(const fVec& vec1, const fVec& vec2);
625  void add(const fVec& vec);
626  void sub(const fVec& vec1, const fVec& vec2);
627  void div(const fVec& vec, double d);
628  void mul(const fVec& vec, double d);
629  void mul(double d, const fVec& vec);
631  void mul(const fMat& mat, const fVec& vec);
633  void mul(const fVec& vec, const fMat& mat);
635 protected:
636 };
637 
638 #endif
~fMat()
Destructor.
Definition: fMatrix.h:120
int inv_row_replaced(const fMat &P, const fMat &q, const fMat &m2d, fMat &X, fMat &y)
Definition: fMatrix.cpp:1334
int resize(int i, int j)
Changes the matrix size.
Definition: fMatrix.h:133
fMat()
Default constructor.
Definition: fMatrix.h:53
fEulerPara operator*(double d, const fEulerPara &ep)
Definition: fEulerPara.cpp:261
fMat eigs(const fMat &mat, double *w)
Definition: fMatrix.cpp:1659
Definition: clap.cpp:13
double sum()
Returns the sum of the elements.
Definition: fMatrix.h:569
fMat lineq_svd(const fMat &A, const fMat &b, int lwork=-1)
Definition: fMatrix.cpp:416
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
int col() const
Returns the number of columns.
Definition: fMatrix.h:154
int m_info
Definition: fMatrix.h:482
int inv_shrink(const fMat &P, const fMat &q, const fMat &r, const fMat &s, fMat &X)
Definition: fMatrix.cpp:1318
fVec(int m, double ini)
Definition: fMatrix.h:499
double length() const
Length of the vector.
Definition: fMatrix.h:553
w
png_bytep png_bytep png_size_t length
Definition: png.h:1541
png_uint_32 i
Definition: png.h:2735
Comprehensive wrapper for CLAPACK functions.
long b
Definition: jpegint.h:371
fMat lineq_sr(const fMat &A, fVec &w_err, fVec &w_norm, double k, const fMat &b)
Definition: fMatrix.cpp:930
friend fVec unit(const fVec &v)
Returns a unit vector with the same direction (no length check!).
Definition: fMatrix.h:557
double det(const dmatrix &_a)
fMat operator+(const fMat &mat1, const fMat &mat2)
Definition: fMatrix.cpp:1187
void zero()
Creates a zero matrix.
Definition: fMatrix.h:249
fMat(int m, int n, double *ini=0)
Constructor with size and initial values (optional).
Definition: fMatrix.h:67
double * p_data
Definition: fMatrix.h:480
fMat(const fMat &ini)
Copy constructor.
Definition: fMatrix.h:102
fVec(const fVec &ini)
Definition: fMatrix.h:501
double temp
Definition: fMatrix.h:481
t
fMat(int m, int n, double ini)
Constructor with size and common initial values.
Definition: fMatrix.h:87
void unit()
Converts to a unit vector (no length check!).
Definition: fMatrix.h:564
void resize(int i)
Change the size.
Definition: fMatrix.h:511
std::ostream & operator<<(std::ostream &ost, const Point &p)
Definition: DistFuncs.cpp:3
Vector of generic size.
Definition: fMatrix.h:491
int row() const
Returns the number of rows.
Definition: fMatrix.h:158
png_size_t start
Definition: png.h:1496
fMat lineq(const fMat &A, const fMat &b)
Definition: fMatrix.cpp:137
fMat sr_inv_svd(const fMat &mat, fVec &w_err, fVec &w_norm, double k, int lwork=-1)
Definition: fMatrix.cpp:536
fMat tran(const fMat &mat)
Definition: fMatrix.cpp:1013
fMat inv(const fMat &mat)
Definition: fMatrix.cpp:176
void zero()
Creates a zero vector.
Definition: fMatrix.h:575
fVec(int m, double *ini=0)
Definition: fMatrix.h:497
int inv_enlarge(const fMat &m12, const fMat &m21, const fMat &m22, const fMat &P, fMat &X, fMat &y, fMat &z, fMat &w)
Definition: fMatrix.cpp:1288
fMat operator/(const fMat &mat, double d)
Definition: fMatrix.cpp:1435
fMat inv_svd(const fMat &mat, int lwork=-1)
Definition: fMatrix.cpp:484
int n_col
Definition: fMatrix.h:483
int n_row
Definition: fMatrix.h:484
friend double length(const fVec &v)
Length of the vector.
Definition: fMatrix.h:549
fMat p_inv(const fMat &mat)
Definition: fMatrix.cpp:191
int inv_col_replaced(const fMat &P, const fMat &q, const fMat &m2d, fMat &X, fMat &y)
Definition: fMatrix.cpp:1357
fVec()
Definition: fMatrix.h:495
fMat p_inv_svd(const fMat &mat, int lwork=-1)
Definition: fMatrix.cpp:501
* y
Definition: IceUtils.h:97
void setinfo(int _info)
Sets m_info.
Definition: fMatrix.h:166
int info()
Returns the value of m_info.
Definition: fMatrix.h:162
double * data() const
Returns the pointer to the first element.
Definition: fMatrix.h:193
fEulerPara operator-(const fEulerPara &_ep)
Definition: fEulerPara.cpp:310
int size() const
Size of the vector (same as row()).
Definition: fMatrix.h:507
int dims_copy(double *_x, double *_y, int _n)
Wrappers of BLAS functions.
Matrix of generic size. The elements are stored in a one-dimensional array in row-major order...
Definition: fMatrix.h:46
fMat sr_inv(const fMat &mat, fVec &w_err, fVec &w_norm, double k)
Definition: fMatrix.cpp:226
~fVec()
Definition: fMatrix.h:503


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:03