Go to the documentation of this file.
34 assert(row_start >= 0 && col_start >= 0 &&
39 double* m = allM.
data() + allM.
row()*(
i+col_start) + row_start;
41 for(
int j=0; j<
n_row; j++, p++, m++)
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++)
56 double* m = subM.
data() +
i*subM.
row();
58 for(
int j=0; j<row_span; j++, p++, m++)
78 int row_span = subV.
size();
80 double* m = subV.
data();
82 for(
int i=0;
i<row_span;
i++, p++, m++)
139 assert(A.
row() ==
b.row());
145 int n = A.
row(), nrhs =
b.col();
158 int n = A.
row(), nrhs =
b.col();
171 int n = A.
row(), nrhs = 1;
178 assert(mat.
row() == mat.
col());
181 int n = mat.
row(), nrhs = mat.
row();
205 ret.
mul(tmat, MMinv);
208 else if(mat.
row() > mat.
col())
216 ret.
mul(MMinv, tmat);
228 assert(w_err.
row() == mat.
row() && w_norm.
row() == mat.
col());
244 Jhat(
i, j) *= w_err(
i);
255 Jhat(
i, j) /= w_norm(j);
259 static fMat tmp, minv;
264 tmp.
mul(Jhat, tJhat);
270 ret.
mul(tJhat, minv);
277 tmp.
mul(tJhat, Jhat);
283 ret.
mul(minv, tJhat);
288 for(j=0; j<mat.
n_row; j++)
290 ret(
i, j) *= w_err(j) / w_norm(
i);
300 int n = mat.
row(), nrhs = mat.
row();
329 else if(mat.
row() > mat.
col())
361 Jhat(
i, j) *= w_err(
i);
372 Jhat(
i, j) /= w_norm(j);
381 tmp.
mul(Jhat, tJhat);
394 tmp.
mul(tJhat, Jhat);
405 for(j=0; j<mat.
n_row; j++)
407 (*this)(
i, j) *= w_err(j) / w_norm(
i);
418 assert(A.
row() ==
b.row());
420 double *
a, *bb, *xx, *s;
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];
439 AA.mul_tran(A,
true);
445 else if(A.
row() == 1)
449 AA.mul_tran(A,
false);
455 double *
a, *bb, *xx, *s;
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];
471 double *
a, *bb, *xx, *s;
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];
486 assert(mat.
row() == mat.
col());
488 double *
a, *
b, *x, *s;
489 int m = mat.
row(),
n = mat.
col(), nrhs = mat.
row(), rank;
505 static fMat MM, MMinv;
515 ret.
mul(MMinv, tmat);
518 else if(mat.
row() > mat.
col())
526 ret.
mul(tmat, MMinv);
538 assert(w_err.
row() == mat.
row() && w_norm.
row() == mat.
col());
540 static fMat Jhat, tJhat;
551 for(j=0; j<mat.
n_col; j++)
553 Jhat(
i, j) *= w_err(
i);
556 for(j=0; j<mat.
n_col; j++)
564 Jhat(
i, j) *= w_norm(j);
568 static fMat tmp, minv;
573 tmp.
mul(Jhat, tJhat);
579 ret.
mul(tJhat, minv);
586 tmp.
mul(tJhat, Jhat);
592 ret.
mul(minv, tJhat);
597 for(j=0; j<mat.
n_row; j++)
599 ret(
i, j) *= w_err(j) / w_norm(
i);
614 double *
a, *
b, *x, *s;
615 int m = mat.
row(),
n = mat.
col(), nrhs = mat.
row(), rank;
621 if(m<
n) s =
new double[m];
622 else s =
new double[
n];
646 else if(mat.
row() > mat.
col())
677 for(j=0; j<mat.
n_col; j++)
679 Jhat(
i, j) *= w_err(
i);
682 for(j=0; j<mat.
n_col; j++)
690 Jhat(
i, j) *= w_norm(j);
700 tmp.
mul(Jhat, tJhat);
714 tmp.
mul(tJhat, Jhat);
725 for(j=0; j<mat.
n_row; j++)
727 (*this)(
i, j) *= w_err(j) / w_norm(
i);
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);
810 for(
i=0;
i<A_row;
i++)
812 for(j=0; j<A_col; j++)
814 Jhat(
i, j) *= w_err(
i);
815 Jhat(
i, j) /= w_norm(j);
819 for(
i = 0;
i < A_row;
i++)
821 for(j = 0; j < n_rhs; j++)
823 wb(
i, j) *= w_err(
i);
826 fMat tmp, tmpx, tmpb;
829 tmpx.
resize(A_row, n_rhs);
832 for(
i=0;
i<A_row;
i++)
839 for(
i=0;
i<A_col;
i++)
841 for(j = 0; j < n_rhs; j++){
842 (*this)(
i, j) /= w_norm(
i);
849 tmpb.
resize(A_col, n_rhs);
851 for(
i=0;
i<A_col;
i++)
856 (*this).lineq_posv(tmp, tmpb);
858 for(
i = 0;
i < A_col;
i++)
860 for(j = 0; j < n_rhs; j++)
862 (*this)(
i, j) /= w_norm(
i);
872 w_norm.
row() == A.
col() &&
b.row() == A.
row());
875 int A_row = A.
row(), A_col = A.
col();
876 Jhat.
resize(A_row, A_col);
882 for(
i=0;
i<A_row;
i++)
884 for(j=0; j<A_col; j++)
886 pp = Jhat.
data() +
i + A_row*j;
887 *pp *= w_err(
i) / w_norm(j);
898 for(
i=0;
i<A_row;
i++)
905 for(
i=0;
i<A_col;
i++)
907 (*this)(
i) /= w_norm(
i);
915 for(
i=0;
i<A_col;
i++)
922 for(
i = 0;
i < A_col;
i++)
924 (*this)(
i) /= w_norm(
i);
932 assert(w_err.
row() == A.
row() && w_norm.
row() == A.
col() &&
935 fMat Jhat, tJhat, wb;
948 for(j=0; j<A.
col(); j++)
950 Jhat(
i, j) *= w_err(
i);
953 for(j=0; j<A.
col(); j++)
961 Jhat(
i, j) /= w_norm(j);
965 for(
i = 0;
i < A.
row();
i++){
966 for(j = 0; j <
b.col(); j++){
967 wb(
i, j) *= w_err(
i);
970 fMat tmp, tmpx, tmpb;
975 tmp.
mul(Jhat, tJhat);
984 for(j = 0; j <
b.col(); j++)
986 x(
i, j) /= w_norm(
i);
995 tmp.
mul(tJhat, Jhat);
1000 tmpb.
mul(tJhat, wb);
1004 for(j = 0; j <
b.col(); j++)
1006 x(
i, j) /= w_norm(
i);
1031 for(
i=0,
n=0;
i<
c;
i++,
n+=r)
1033 for(j=0, m=0; j<r; j++, m+=
c)
1044 int i, j, m,
n, idx1, idx2;
1221 int i, j, k,
n,
c = mat1.
col(), r = mat1.
row();
1222 double* p1, *p2, *pp;
1236 int m = mat1.
row(), k = mat1.
col(),
n = mat2.
col();
1247 int i, j, r = mat1.
row();
1248 double* p1 = mat1.
p_data, *p2, *pp;
1253 for(
i=0, p1=mat1.
p_data;
i<=j;
i++, p1+=r, pp++)
1273 for(
i=0;
i<ret.
row();
i++)
1275 for(j=0; j<ret.
col(); j++)
1279 for(k=0; k<mat1.
col(); k++)
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);
1320 int n_org = P.
col();
1321 int n_add = q.
col();
1326 fMat sr(n_add, n_org), qsr(n_org, n_org);
1329 X.resize(n_org, n_org);
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);
1348 m2d_q_inv.inv_svd(m2d_q);
1349 y.
mul(q, m2d_q_inv);
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);
1371 q_m2d_inv.inv_svd(q_m2d);
1372 y.
mul(q_m2d_inv, q);
1384 int i, k,
c = mat.
col(), r = mat.
row();
1385 double* pm, *pv, *pp;
1389 for(k=0, pm=mat.
data()+
i, pv=vec.
data(); k<
c; k++, pm+=r, pv++)
1396 int c = mat.
col(), r = mat.
row();
1404 int i, r = mat.
row();
1405 double* pm, *pv = vec.
p_data, *pp;
1414 assert(mat.
col() == vec.
row());
1417 for(
i=0;
i<ret.
row();
i++)
1420 for(k=0; k<mat.
col(); k++)
1449 double max_val = 0.0;
1453 if(ret < 0 ||
p_data[
i] > max_val)
1465 double min_val = 0.0;
1469 if(ret < 0 ||
p_data[
i] < min_val)
1481 double max_val = 0.0;
1485 if(index < 0 ||
p_data[
i] > max_val)
1497 double min_val = 0.0;
1501 if(index < 0 ||
p_data[
i] < min_val)
1664 a=
new double [mat.
col()*mat.
row()];
1668 for(
i=0;
i<mat.
row();
i++){
1669 for(j=0;j<mat.
col();j++){
1732 cerr <<
"fMat::dmul(fMat, fMat): matrix size error" << endl;
1781 cerr <<
"fMat::dmul(fMat, fMat): matrix size error" << endl;
1790 cerr <<
"matrix size error at function det()" << endl;
1806 cerr <<
"matrix size error at function eig()" << endl;
1826 cerr <<
"matrix size error at function eig()" << endl;
1848 cerr <<
"matrix size error at function eig()" << endl;
1853 double *p_wi, *p_vr, *p_vi;
1867 for(j=0;j<
n_row;j++)
1872 for(j=0;j<
n_row;j++)
1873 *(p_vi+j) = *(p_vr+j);
1876 for(j=0;j<
n_row;j++)
1877 *(p_vi+j) = -*(p_vr+j);
1879 for(j=0;j<
n_row;j++)
1880 *(p_vr+j) = *(p_vr+j-
n_row);
void set(double *_d)
Sets all elements.
void symmetric(char t='U')
Change to symmetric matrix.
double min_value()
Returns the minimum value.
int max_index()
Returns the index of the largest element.
fMat p_inv_svd(const fMat &mat, int lwork)
void div(const fMat &mat, double d)
void identity()
Creates an identity matrix (only for square matrices).
void operator/=(double d)
friend fMat sr_inv_svd(const fMat &mat, fVec &w_err, fVec &w_norm, double k, int lwork)
SR-inverse.
fMat eigs(const fMat &mat, double *w)
int eig(fVec &wr, fVec &wi)
Computes the eigenvalues.
int lineq_porfs(const fMat &A, const fMat &b)
solve linear equation Ax = b, where A is positive-definite, symmetric
friend fMat inv(const fMat &mat)
inverse
int dims_dgeev(int _n, double *_a, double *_wr, double *_wi, double *_vr)
Computes eigenvalues and eigenvectors.
fMat sr_inv(const fMat &mat, fVec &w_err, fVec &w_norm, double k)
int inv_posv(const fMat &)
inverse of positive-definite, symmetric matrix
int lineq_sr(const fMat &A, fVec &w_err, fVec &w_norm, double k, const fVec &b)
int size() const
Size of the vector (same as row()).
fMat()
Default constructor.
void ddiv(const fMat &mat1, const fMat &mat2)
Element-wise division: ./ operator in MATLAB.
int lineq_posv(const fMat &A, const fMat &b)
solve linear equation Ax = b, where A is positive-definite, symmetric
int dims_copy(double *_x, double *_y, int _n)
Wrappers of BLAS functions.
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
int dims_svd(double *_a, int _m, int _n, double *_u, double *_sigma, double *_vt)
Performs singular value decomposition.
void set_subvec(int start, const fVec &subV)
Copy a smaller vector as a sub-vector.
int dims_scale(double *_x, double _alpha, int _n, double *_y)
fMat p_inv(const fMat &mat)
void operator=(double d)
Assignment operations.
double * data() const
Returns the pointer to the first element.
void operator/=(double d)
int inv_row_replaced(const fMat &P, const fMat &q, const fMat &m2d, fMat &X, fMat &y)
int dims_dsyrk(double *_A, int _n, int _k, double *_C)
void add(const fVec &vec1, const fVec &vec2)
void mul(const fVec &vec, double d)
#define MIN(a, b)
Returns the min value between a and b.
friend fMat tran(const fMat &mat)
Returns the transpose of a matrix.
void mul(const fMat &mat1, const fMat &mat2)
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.
int resize(int i, int j)
Changes the matrix size.
void div(const fVec &vec, double d)
fMat inv_svd(const fMat &mat, int lwork)
int inv_porfs(const fMat &)
inverse of positive-definite, symmetric matrix
int inv_enlarge(const fMat &m12, const fMat &m21, const fMat &m22, const fMat &P, fMat &X, fMat &y, fMat &z, fMat &w)
double dims_dot(double *_x, double *_y, int _n)
void operator*=(double d)
void set(double *_d)
Sets the values from an array.
void get_subvec(int start, const fVec &allV)
Copy a sub-vector of a larger vector.
friend fMat lineq_svd(const fMat &A, const fMat &b, int lwork)
solve linear equation Ax = b
fMat lineq(const fMat &A, const fMat &b)
void mul_tran(const fMat &mat1, int trans_first)
void operator-=(const fMat &mat)
fMat inv(const fMat &mat)
void operator+=(const fMat &mat)
int dims_dgesvx(double *_a, double *_x, double *_b, int _n, int _nrhs)
Solves linear equation using LU decomposition.
void sub(const fVec &vec1, const fVec &vec2)
void operator+=(const fVec &vec)
void abs()
Converts all elements to their absolute values.
fMat operator=(const fMat &mat)
Assignment from a reference matrix.
int svd(fMat &U, fVec &Sigma, fMat &VT)
singular value decomposition
fMat lineq_sr(const fMat &A, fVec &w_err, fVec &w_norm, double k, const fMat &b)
int lineq(const fMat &A, const fVec &b)
Solves linear equation Ax = b.
friend fMat lineq(const fMat &A, const fMat &b)
solve linear equation Ax = b
fMat operator/(const fMat &mat, double d)
int inv_shrink(const fMat &P, const fMat &q, const fMat &r, const fMat &s, fMat &X)
void neg(const fMat &mat)
int min_index()
Returns the index of the smallest element.
void resize(int i)
Change the size.
int inv_col_replaced(const fMat &P, const fMat &q, const fMat &m2d, fMat &X, fMat &y)
int dims_dposv(double *_a, double *_x, double *_b, int _m, int _nrhs)
void set_submat(int row_start, int col_start, const fMat &subM)
Sets a sub-matrix of myself.
int row() const
Returns the number of rows.
fMat tran(const fMat &mat)
void add(const fMat &mat1, const fMat &mat2)
int dims_dgeev_simple(int _n, double *_a, double *_wr, double *_wi)
Computes eigenvalues only.
void operator-=(const fVec &vec)
double det(void)
Computes the determinant.
int dims_dporfs(double *_a, double *_x, double *_b, int _m, int _nrhs)
For positive-definite, symmetric matrices.
void sub(const fMat &mat1, const fMat &mat2)
friend fMat p_inv_svd(const fMat &mat, int lwork)
pseudo inverse
void get_submat(int row_start, int col_start, const fMat &allM)
Extract a sub-matrix and set to myself.
fMat operator*(double d, const fMat &mat)
int lineq_porfs(const fMat &A, const fVec &b)
fMat operator+(const fMat &mat1, const fMat &mat2)
friend fMat inv_svd(const fMat &mat, int lwork)
inverse
int col() const
Returns the number of columns.
friend fMat p_inv(const fMat &mat)
pseudo inverse
fMat operator-(const fMat &mat)
int dims_daxpy(int _n, double _alpha, double *_x, double *_y)
Generic matrix/vector classes.
void tran()
Transposes a matrix (only for square matrices).
void operator*=(double d)
int dims_dgemm(double *_A, double *_B, int _m, int _n, int _k, double *_C)
void dmul(const fMat &mat1, const fMat &mat2)
Element-wise multiplication: .* operator in MATLAB.
fMat lineq_svd(const fMat &A, const fMat &b, int lwork)
int dims_dgemv(double *_A, int _m, int _n, double *_x, double *_y)
int dims_eigs(int _n, double *_a, double *w)
Eigenvalues / eigenvectors.
int lineq_posv(const fMat &A, const fVec &b)
int dims_det(int _n, double *_a, double *_x)
Computes the determinant.
void neg(const fVec &vec)
friend fMat sr_inv(const fMat &mat, fVec &w_err, fVec &w_norm, double k)
singularity-robust (SR) inverse
int lineq_svd(const fMat &A, const fVec &b, int lwork=-1)
double max_value()
Returns the maximum value.
fMat sr_inv_svd(const fMat &mat, fVec &w_err, fVec &w_norm, double k, int lwork)
int dims_scale_myself(double *_x, double _alpha, int _n)
openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Wed Sep 7 2022 02:51:02