void eqnsys<nr_type_t>::factorize_lu_crout (void) {
nr_double_t d, MaxPivot;
nr_type_t f;
int k, c, r, pivot;
// initialize pivot exchange table
for (r = 0; r < N; r++) {
for (MaxPivot = 0, c = 0; c < N; c++)
if ((d = abs (A_(r, c))) > MaxPivot)
MaxPivot = d;
if (MaxPivot <= 0) MaxPivot = NR_TINY;
nPvt[r] = 1 / MaxPivot;
rMap[r] = r;
}
// decompose the matrix into L (lower) and U (upper) matrix
for (c = 0; c < N; c++) {
// upper matrix entries
for (r = 0; r < c; r++) {
f = A_(r, c);
for (k = 0; k < r; k++) f -= A_(r, k) * A_(k, c);
A_(r, c) = f / A_(r, r);
}
// lower matrix entries
for (MaxPivot = 0, pivot = r; r < N; r++) {
f = A_(r, c);
for (k = 0; k < c; k++) f -= A_(r, k) * A_(k, c);
A_(r, c) = f;
// larger pivot ?
if ((d = nPvt[r] * abs (f)) > MaxPivot) {
MaxPivot = d;
pivot = r;
}
}
// check pivot element and throw appropriate exception
if (MaxPivot <= 0) {
#if LU_FAILURE
qucs::exception * e = new qucs::exception (EXCEPTION_PIVOT);
e->setText ("no pivot != 0 found during Crout LU decomposition");
e->setData (c);
throw_exception (e);
goto fail;
#else /* insert virtual resistance */
VIRTUAL_RES ("no pivot != 0 found during Crout LU decomposition", c);
#endif
}
// swap matrix rows if necessary and remember that step in the
// exchange table
if (c != pivot) {
A->exchangeRows (c, pivot);
Swap (int, rMap[c], rMap[pivot]);
Swap (nr_double_t, nPvt[c], nPvt[pivot]);
}
}
#if LU_FAILURE
fail:
#endif
}
void eqnsys<nr_type_t>::ensure_diagonal_MNA (void) {
int restart, exchanged, begin = 0, pairs;
int pivot1, pivot2, i;
do {
restart = exchanged = 0;
/* search for zero diagonals with lone pairs */
for (i = begin; i < N; i++) {
if (A_(i, i) == 0) {
pairs = countPairs (i, pivot1, pivot2);
if (pairs == 1) { /* lone pair found, substitute rows */
A->exchangeRows (i, pivot1);
B->exchangeRows (i, pivot1);
exchanged = 1;
}
else if ((pairs > 1) && !restart) {
restart = 1;
begin = i;
}
}
}
/* all lone pairs are gone, look for zero diagonals with multiple pairs */
if (restart) {
for (i = begin; !exchanged && i < N; i++) {
if (A_(i, i) == 0) {
pairs = countPairs (i, pivot1, pivot2);
A->exchangeRows (i, pivot1);
B->exchangeRows (i, pivot1);
exchanged = 1;
}
}
}
}
while (restart);
}
void eqnsys<nr_type_t>::substitute_qr_householder_ls (void) {
int c, r;
nr_type_t f;
// forward substitution in order to solve R'X = B
for (r = 0; r < N; r++) {
for (f = B_(r), c = 0; c < r; c++) f -= A_(c, r) * B_(c);
if (abs (A_(r, r)) > std::numeric_limits<nr_double_t>::epsilon())
B_(r) = f / A_(r, r);
else
B_(r) = 0;
}
// compute the least square solution QX
for (c = N - 1; c >= 0; c--) {
if (T_(c) != 0) {
// scalar product u' * B
for (f = B_(c), r = c + 1; r < N; r++) f += cond_conj (A_(r, c)) * B_(r);
// z - T * f * u_k
f *= T_(c); B_(c) -= f;
for (r = c + 1; r < N; r++) B_(r) -= f * A_(r, c);
}
}
// permute solution vector
for (r = 0; r < N; r++) X_(cMap[r]) = B_(r);
}
nr_double_t eqnsys<nr_type_t>::euclidian_r (int r, int c) {
nr_double_t scale = 0, n = 1;
for (int i = c; i < N; i++) {
euclidian_update (real (A_(r, i)), n, scale);
euclidian_update (imag (A_(r, i)), n, scale);
}
return scale * sqrt (n);
}
void HumanoidFootConstraint<Scalar>::computeGeneralConstraintsMatrices(int sizeVec)
{
computeNbGeneralConstraints();
int N = feetSupervisor_.getNbSamples();
int M = feetSupervisor_.getNbPreviewedSteps();
A_.setZero(nbGeneralConstraints_,sizeVec);
b_.setConstant(nbGeneralConstraints_, Constant<Scalar>::MAXIMUM_BOUND_VALUE);
//Number of general constraints at current step
int nbGeneralConstraintsAtCurrentStep(0);
//Sum of general constraints numbers since first step
int nbGeneralConstraintsSinceFirstStep(0);
for(int i = 0; i<M; ++i)
{
const ConvexPolygon<Scalar>& cp = feetSupervisor_.getKinematicConvexPolygon(i);
nbGeneralConstraintsAtCurrentStep = cp.getNbGeneralConstraints();
for(int j = 0; j<nbGeneralConstraintsAtCurrentStep; ++j)
{
// Filling matrix A and vector b to create a general constraint of the form
// AX + b <=0
A_(nbGeneralConstraintsSinceFirstStep + j, 2*N + i) =
cp.getGeneralConstraintsMatrixCoefsForX()(j);
A_(nbGeneralConstraintsSinceFirstStep + j, 2*N + M + i) =
cp.getGeneralConstraintsMatrixCoefsForY()(j);
b_(nbGeneralConstraintsSinceFirstStep + j) =
cp.getGeneralConstraintsConstantPart()(j);
// This if-else statement adds the required terms to matrices A and b so that
// constraints are expressed in world frame
if(i==0)
{
b_(nbGeneralConstraintsSinceFirstStep + j) -=
cp.getGeneralConstraintsMatrixCoefsForX()(j)
*feetSupervisor_.getSupportFootStateX()(0)
+ cp.getGeneralConstraintsMatrixCoefsForY()(j)
*feetSupervisor_.getSupportFootStateY()(0);
}
else
{
A_(nbGeneralConstraintsSinceFirstStep + j, 2*N + i - 1) -=
cp.getGeneralConstraintsMatrixCoefsForX()(j);
A_(nbGeneralConstraintsSinceFirstStep + j, 2*N + i - 1 + M) -=
cp.getGeneralConstraintsMatrixCoefsForY()(j);
}
}
nbGeneralConstraintsSinceFirstStep += nbGeneralConstraintsAtCurrentStep;
}
}
nr_double_t eqnsys<nr_type_t>::convergence_criteria (void) {
nr_double_t f = 0;
for (int r = 0; r < A->getCols (); r++) {
for (int c = 0; c < A->getCols (); c++) {
if (r != c) f += norm (A_(r, c) / A_(r, r));
}
}
return sqrt (f);
}
BiCGStabResult BiCGstab::solve(const Array& b, const Array& x0) const {
Real bnorm2 = Norm2(b);
if (bnorm2 == 0.0) {
BiCGStabResult result = { 0, 0.0, b};
return result;
}
Array x = ((!x0.empty()) ? x0 : Array(b.size(), 0.0));
Array r = b - A_(x);
Array rTld = r;
Array p, pTld, v, s, sTld, t;
Real omega = 1.0;
Real rho, rhoTld=1.0;
Real alpha = 0.0, beta;
Real error = Norm2(r)/bnorm2;
Size i;
for (i=0; i < maxIter_ && error >= relTol_; ++i) {
rho = DotProduct(rTld, r);
if (rho == 0.0 || omega == 0.0)
break;
if (i) {
beta = (rho/rhoTld)*(alpha/omega);
p = r + beta*(p - omega*v);
}
else {
p = r;
}
pTld = ((M_)? M_(p) : p);
v = A_(pTld);
alpha = rho/DotProduct(rTld, v);
s = r-alpha*v;
if (Norm2(s) < relTol_*bnorm2) {
x += alpha*pTld;
error = Norm2(s)/bnorm2;
break;
}
sTld = ((M_) ? M_(s) : s);
t = A_(sTld);
omega = DotProduct(t,s)/DotProduct(t,t);
x += alpha*pTld + omega*sTld;
r = s - omega*t;
error = Norm2(r)/bnorm2;
rhoTld = rho;
}
QL_REQUIRE(i < maxIter_, "max number of iterations exceeded");
QL_REQUIRE(error < relTol_, "could not converge");
BiCGStabResult result = { i, error, x};
return result;
}
void eqnsys<nr_type_t>::householder_apply_right_extern (int r, nr_type_t t) {
nr_type_t f;
int k, c;
// apply the householder vector to each downward row
for (c = r + 1; c < N; c++) {
// calculate f = u' * A (a scalar product)
f = V_(c, r + 1);
for (k = r + 2; k < N; k++) f += cond_conj (A_(r, k)) * V_(c, k);
// calculate A -= T * f * u
f *= cond_conj (t); V_(c, r + 1) -= f;
for (k = r + 2; k < N; k++) V_(c, k) -= f * A_(r, k);
}
}
void eqnsys<nr_type_t>::solve_gauss (void) {
nr_double_t MaxPivot;
nr_type_t f;
int i, c, r, pivot;
// triangulate the matrix
for (i = 0; i < N; i++) {
// find maximum column value for pivoting
for (MaxPivot = 0, pivot = r = i; r < N; r++) {
if (abs (A_(r, i)) > MaxPivot) {
MaxPivot = abs (A_(r, i));
pivot = r;
}
}
// exchange rows if necessary
assert (MaxPivot != 0);
if (i != pivot) {
A->exchangeRows (i, pivot);
B->exchangeRows (i, pivot);
}
// compute new rows and columns
for (r = i + 1; r < N; r++) {
f = A_(r, i) / A_(i, i);
for (c = i + 1; c < N; c++) A_(r, c) -= f * A_(i, c);
B_(r) -= f * B_(i);
}
}
// backward substitution
for (i = N - 1; i >= 0; i--) {
f = B_(i);
for (c = i + 1; c < N; c++) f -= A_(i, c) * X_(c);
X_(i) = f / A_(i, i);
}
}
/* ----------------------------------------------------------------
* 功 能：ANSI X9.8 加密模块
* 输入参数：uszKey 加密密钥明文
* szPan 账号
* szPasswd 密码明文
* iPwdLen 密码明文长度
* iFlag 算法标识: 采用DES或3DES
* 输出参数：uszResult 采用ansi98加密后的密码密文
* 返 回 值：-1 加密失败； 0 成功
* 作 者：
* 日 期：
* 调用说明：
* 修改日志：修改日期 修改者 修改内容简述
* ----------------------------------------------------------------
*/
int ANSIX98(unsigned char *uszKey, char *szPan, char *szPwd,
int iPwdLen, int iFlag, unsigned char *uszResult)
{
int i;
int iRet;
unsigned char password[9];
if (iPwdLen > 8)
{
return FAIL;
}
iRet = A_(szPwd, szPan, iPwdLen, uszResult);
if (iRet < 0 )
{
return FAIL;
}
if (iFlag == TRIPLE_DES)
{
TriDES( uszKey, uszResult, password );
}
else
{
DES(uszKey, uszResult, password);
}
memcpy((char*) uszResult, (char*) password, 8);
uszResult[8] = '\0';
return SUCC;
}
int eqnsys<nr_type_t>::countPairs (int i, int& r1, int& r2) {
int pairs = 0;
for (int r = 0; r < N; r++) {
if (fabs (real (A_(r, i))) == 1.0) {
r1 = r;
pairs++;
for (r++; r < N; r++) {
if (fabs (real (A_(r, i))) == 1.0) {
r2 = r;
if (++pairs >= 2) return pairs;
}
}
}
}
return pairs;
}
Vector Solver::Q( const Vector& r ) const
{
auto Qr = P_( r );
for ( auto i = 0u; i < getIterativeRefinements(); ++i )
Qr += P_( r - A_( Qr ) );
return Qr;
}
bool RKButcherTableauBase<Scalar>::operator== (const RKButcherTableauBase<Scalar>& rkbt) const
{
if (this->numStages() != rkbt.numStages()) {
return false;
}
if (this->order() != rkbt.order()) {
return false;
}
int N = rkbt.numStages();
// Check b and c first:
const Teuchos::SerialDenseVector<int,Scalar> b_ = this->b();
const Teuchos::SerialDenseVector<int,Scalar> c_ = this->c();
const Teuchos::SerialDenseVector<int,Scalar> other_b = rkbt.b();
const Teuchos::SerialDenseVector<int,Scalar> other_c = rkbt.c();
for (int i=0 ; i<N ; ++i) {
if (b_(i) != other_b(i)) {
return false;
}
if (c_(i) != other_c(i)) {
return false;
}
}
// Then check A:
const Teuchos::SerialDenseMatrix<int,Scalar>& A_ = this->A();
const Teuchos::SerialDenseMatrix<int,Scalar>& other_A = rkbt.A();
for (int i=0 ; i<N ; ++i) {
for (int j=0 ; j<N ; ++j) {
if (A_(i,j) != other_A(i,j)) {
return false;
}
}
}
return true;
}
void eqnsys<nr_type_t>::factorize_qrh (void) {
int c, r, k, pivot;
nr_type_t f, t;
nr_double_t s, MaxPivot;
delete R; R = new tvector<nr_type_t> (N);
for (c = 0; c < N; c++) {
// compute column norms and save in work array
nPvt[c] = euclidian_c (c);
cMap[c] = c; // initialize permutation vector
}
for (c = 0; c < N; c++) {
// put column with largest norm into pivot position
MaxPivot = nPvt[c]; pivot = c;
for (r = c + 1; r < N; r++) {
if ((s = nPvt[r]) > MaxPivot) {
pivot = r;
MaxPivot = s;
}
}
if (pivot != c) {
A->exchangeCols (pivot, c);
Swap (int, cMap[pivot], cMap[c]);
Swap (nr_double_t, nPvt[pivot], nPvt[c]);
}
// compute householder vector
if (c < N) {
nr_type_t a, b;
s = euclidian_c (c, c + 1);
a = A_(c, c);
b = -sign (a) * xhypot (a, s); // Wj
t = xhypot (s, a - b); // || Vi - Wi ||
R_(c) = b;
// householder vector entries Ui
A_(c, c) = (a - b) / t;
for (r = c + 1; r < N; r++) A_(r, c) /= t;
}
else {
R_(c) = A_(c, c);
}
// apply householder transformation to remaining columns
for (r = c + 1; r < N; r++) {
for (f = 0, k = c; k < N; k++) f += cond_conj (A_(k, c)) * A_(k, r);
for (k = c; k < N; k++) A_(k, r) -= 2.0 * f * A_(k, c);
}
// update norms of remaining columns too
for (r = c + 1; r < N; r++) {
nPvt[r] = euclidian_c (r, c + 1);
}
}
}
double Lasso(size_t m, size_t n) {
std::vector<T> A(m * n);
std::vector<T> b(m);
std::default_random_engine generator;
std::uniform_real_distribution<T> u_dist(static_cast<T>(0),
static_cast<T>(1));
std::normal_distribution<T> n_dist(static_cast<T>(0),
static_cast<T>(1));
for (unsigned int i = 0; i < m * n; ++i)
A[i] = n_dist(generator);
std::vector<T> x_true(n);
for (unsigned int i = 0; i < n; ++i)
x_true[i] = u_dist(generator) < static_cast<T>(0.8)
? static_cast<T>(0) : n_dist(generator) / static_cast<T>(std::sqrt(n));
#ifdef _OPENMP
#pragma omp parallel for
#endif
for (unsigned int i = 0; i < m; ++i)
for (unsigned int j = 0; j < n; ++j)
b[i] += A[i * n + j] * x_true[j];
// b[i] += A[i + j * m] * x_true[j];
for (unsigned int i = 0; i < m; ++i)
b[i] += static_cast<T>(0.5) * n_dist(generator);
T lambda_max = static_cast<T>(0);
#ifdef _OPENMP
#pragma omp parallel for reduction(max : lambda_max)
#endif
for (unsigned int j = 0; j < n; ++j) {
T u = 0;
for (unsigned int i = 0; i < m; ++i)
//u += A[i * n + j] * b[i];
u += A[i + j * m] * b[i];
lambda_max = std::max(lambda_max, std::abs(u));
}
pogs::MatrixDense<T> A_('r', m, n, A.data());
pogs::PogsDirect<T, pogs::MatrixDense<T> > pogs_data(A_);
std::vector<FunctionObj<T> > f;
std::vector<FunctionObj<T> > g;
f.reserve(m);
for (unsigned int i = 0; i < m; ++i)
f.emplace_back(kSquare, static_cast<T>(1), b[i]);
g.reserve(n);
for (unsigned int i = 0; i < n; ++i)
g.emplace_back(kAbs, static_cast<T>(0.2) * lambda_max);
double t = timer<double>();
pogs_data.Solve(f, g);
return timer<double>() - t;
}
void ArmadilloSolver::solve_irls()
{
// Weight vector
fvec wsq = ones<fmat>(y.n_elem);
//fvec wsq_ = ones<fmat>(y.n_elem);
//fvec err_u = ones<fmat>(y.n_elem/2);
//fvec err_v = ones<fmat>(y.n_elem/2);
fmat A_(A.n_rows, A.n_cols);
//fvec y_(y.n_elem);
fvec x_(A.n_cols);
fmat T1(A.n_cols, A.n_cols);
fvec T2(A.n_cols);
if (this->debug)
{
std::cout << "[DEBUG] Solver dimensions: " << std::endl;
std::cout << "[DEBUG] \t T1: " << T1.n_rows << " x " << T1.n_cols << std::endl;
std::cout << "[DEBUG] \t T2: " << T2.n_rows << " x " << T2.n_cols << std::endl;
std::cout << "[DEBUG] \t A: " << A.n_rows << " x " << A.n_cols << std::endl;
std::cout << "[DEBUG] \t y: " << y.n_elem << std::endl;
std::cout << "[DEBUG] \t x: " << x_.n_elem << std::endl;
std::cout << "[DEBUG] \t Q: " << Q.n_rows << " x " << Q.n_cols << std::endl;
}
int iters = this->n_iters;
for (int iter=0; iter < iters; iter++) {
A_ = A;
// Re-weight rows of matrix and result vector
A_.each_col() %= wsq;
//y_ = y % wsq;
T1 = A_.t() * A_ + Q;
T2 = A_.t() * (y % wsq);
x_ = arma::solve(T1,T2);
// Computing wsq is a little different, since we want to
// weight both x and y direction of the pixel by the L2 error.
wsq = square(A * x_ - y) * 1.0/this->sigma_sq;
wsq = repmat(wsq.subvec(0,n_kp-1) + wsq.subvec(n_kp,2*n_kp-1),2,1);
// Cauchy error
// Note that the error would be 1.0/(1+f**2), but since we do not take the square
// root above, we can omit the squaring here.
for (int j=0; j < wsq.n_elem; j++)
{
wsq[j] = 1.0/(1.0+wsq[j]);
}
//wsq.transform( [](float f) {return 1.0/(1.0+f); } );
wsq = sqrt(wsq);
}
this->x = x_;
this->wsq = square(wsq);
}
static void evaluate(const Shell &A, const Shell &B,
const Shell &C, const Shell &D,
double *Q) {
adapter::rysq::Shell A_(A), B_(B), C_(C), D_(D);
//rysq::Quartet<rysq::Shell> quartet(A_, B_, C_, D_);
boost::array<Center,4> centers = {{
A.center(), B.center(), C.center(), D.center() }};
rysq::Eri eri(rysq::Quartet<rysq::Shell>(A_, B_, C_, D_));
eri(centers, Q);
}
void eqnsys<nr_type_t>::substitute_lu_doolittle (void) {
nr_type_t f;
int i, c;
// forward substitution in order to solve LY = B
for (i = 0; i < N; i++) {
f = B_(rMap[i]);
for (c = 0; c < i; c++) f -= A_(i, c) * X_(c);
// remember that the Lii diagonal are ones only in Doolittle's definition
X_(i) = f;
}
// backward substitution in order to solve UX = Y
for (i = N - 1; i >= 0; i--) {
f = X_(i);
for (c = i + 1; c < N; c++) f -= A_(i, c) * X_(c);
X_(i) = f / A_(i, i);
}
}
void eqnsys<nr_type_t>::preconditioner (void) {
int pivot, r;
nr_double_t MaxPivot;
for (int i = 0; i < N; i++) {
// find maximum column value for pivoting
for (MaxPivot = 0, pivot = i, r = 0; r < N; r++) {
if (abs (A_(r, i)) > MaxPivot &&
abs (A_(i, r)) >= abs (A_(r, r))) {
MaxPivot = abs (A_(r, i));
pivot = r;
}
}
// swap matrix rows if possible
if (i != pivot) {
A->exchangeRows (i, pivot);
B->exchangeRows (i, pivot);
}
}
}
nr_type_t eqnsys<nr_type_t>::householder_create_right (int r) {
nr_type_t a, b, t;
nr_double_t s, g;
s = euclidian_r (r, r + 2);
if (s == 0 && imag (A_(r, r + 1)) == 0) {
// no reflection necessary
t = 0;
}
else {
// calculate householder reflection
a = A_(r, r + 1);
g = sign_(a) * xhypot (a, s);
b = a + g;
t = b / g;
// store householder vector
for (int c = r + 2; c < N; c++) A_(r, c) /= b;
A_(r, r + 1) = -g;
}
return t;
}
nr_type_t eqnsys<nr_type_t>::householder_create_left (int c) {
nr_type_t a, b, t;
nr_double_t s, g;
s = euclidian_c (c, c + 1);
if (s == 0 && imag (A_(c, c)) == 0) {
// no reflection necessary
t = 0;
}
else {
// calculate householder reflection
a = A_(c, c);
g = sign_(a) * xhypot (a, s);
b = a + g;
t = b / g;
// store householder vector
for (int r = c + 1; r < N; r++) A_(r, c) /= b;
A_(c, c) = -g;
}
return t;
}
double LpEq(int m, int n, int nnz) {
std::vector<T> val(nnz);
std::vector<int> col_ind(nnz);
std::vector<int> row_ptr(m + 2);
std::vector<T> x(n);
std::vector<T> y(m + 1);
std::default_random_engine generator;
std::uniform_real_distribution<T> u_dist(static_cast<T>(0),
static_cast<T>(1));
// Enforce c == rand(n, 1)
std::vector<std::tuple<int, int, T>> entries;
entries.reserve(n);
for (int i = 0; i < n; ++i) {
entries.push_back(std::make_tuple(m, i, u_dist(generator)));
}
// Generate A and c according to:
// A = 4 / n * rand(m, n)
nnz = MatGenApprox(m + 1, n, nnz, val.data(), row_ptr.data(), col_ind.data(),
static_cast<T>(0), static_cast<T>(4.0 / n), entries);
pogs::MatrixSparse<T> A_('r', m + 1, n, nnz, val.data(), row_ptr.data(),
col_ind.data());
pogs::PogsIndirect<T, pogs::MatrixSparse<T>> pogs_data(A_);
std::vector<FunctionObj<T> > f;
std::vector<FunctionObj<T> > g;
// Generate b according to:
// v = rand(n, 1)
// b = A * v
std::vector<T> v(n);
for (unsigned int i = 0; i < n; ++i)
v[i] = u_dist(generator);
f.reserve(m + 1);
for (unsigned int i = 0; i < m; ++i) {
T b_i = static_cast<T>(0);
for (unsigned int j = row_ptr[i]; j < row_ptr[i + 1]; ++j)
b_i += val[j] * v[col_ind[j]];
f.emplace_back(kIndEq0, static_cast<T>(1), b_i);
}
f.emplace_back(kIdentity);
g.reserve(n);
for (unsigned int i = 0; i < n; ++i)
g.emplace_back(kIndGe0);
double t = timer<double>();
pogs_data.Solve(f, g);
return timer<double>() - t;
}
bool solveP_( vector< Coef >& x )
{
Coef res, res0;
if ( is_trivial( b_ ) )
{
x = b_;
return true;
}
r_ = b_ - A_ * x;
res = res0 = sync_norm( r_ );
if ( is_trivial( res0 ) ) return true;
for ( int i = 0; i < A_.m(); ++i )
{
bool converged = false;
Coef norm;
norm = x * x;
#ifdef ELAI_DEBUG
std::cerr << " ||x||=" << norm
<< " ||Ax-b||=" << res
<< " " << res / res0 << std::endl;
#endif
if ( std::isnan( res ) ) return false;
else if ( rel_converged( res, res0 ) || abs_converged( res ) ) converged = true;
else if ( !is_trivial( norm ) && rel_converged( res, norm ) ) converged = true;
if ( isOK( converged ) ) return true;
// Preconditionng:
ELAI_PROF_BEG( prec_elapsed_ );
P_->forward( x );
P_->backward( x );
ELAI_SYNC( x );
ELAI_PROF_END( prec_elapsed_ );
for ( int i = 0; i < iter_max(); ++i )
{
Coef a = 1. / A_( i, i );
y_( i ) = a * y_( i ) + x( i );
}
ELAI_SYNC( y_ );
x = y_;
r_ = b_ - A_ * x;
res = sync_norm( r_ );
}
return false;
}
void eqnsys<nr_type_t>::substitute_qrh (void) {
int c, r;
nr_type_t f;
// form the new right hand side Q'B
for (c = 0; c < N - 1; c++) {
// scalar product u_k^T * B
for (f = 0, r = c; r < N; r++) f += cond_conj (A_(r, c)) * B_(r);
// z - 2 * f * u_k
for (r = c; r < N; r++) B_(r) -= 2.0 * f * A_(r, c);
}
// backward substitution in order to solve RX = Q'B
for (r = N - 1; r >= 0; r--) {
f = B_(r);
for (c = r + 1; c < N; c++) f -= A_(r, c) * X_(cMap[c]);
if (abs (R_(r)) > std::numeric_limits<nr_double_t>::epsilon())
X_(cMap[r]) = f / R_(r);
else
X_(cMap[r]) = 0;
}
}
void eqnsys<nr_type_t>::substitute_qr_householder (void) {
int c, r;
nr_type_t f;
// form the new right hand side Q'B
for (c = 0; c < N; c++) {
if (T_(c) != 0) {
// scalar product u' * B
for (f = B_(c), r = c + 1; r < N; r++) f += cond_conj (A_(r, c)) * B_(r);
// z - T * f * u
f *= cond_conj (T_(c)); B_(c) -= f;
for (r = c + 1; r < N; r++) B_(r) -= f * A_(r, c);
}
}
// backward substitution in order to solve RX = Q'B
for (r = N - 1; r >= 0; r--) {
for (f = B_(r), c = r + 1; c < N; c++) f -= A_(r, c) * X_(cMap[c]);
if (abs (A_(r, r)) > std::numeric_limits<nr_double_t>::epsilon())
X_(cMap[r]) = f / A_(r, r);
else
X_(cMap[r]) = 0;
}
}
bool solve_( vector< Coef >& x )
{
Coef res, res0;
if ( is_trivial( b_ ) )
{
x = b_;
return true;
}
r_ = b_ - A_ * x;
res = res0 = sync_norm( r_ );
if ( is_trivial( res0 ) ) return true;
for ( int i = 0; i < iter_max(); ++i )
{
bool converged = false;
Coef norm;
norm = x * x;
#ifdef ELAI_DEBUG
std::cerr << " ||x||=" << norm
<< " ||Ax-b||=" << res
<< " " << res / res0 << std::endl;
#endif
if ( std::isnan( res ) ) return false;
else if ( rel_converged( res, res0 ) || abs_converged( res ) ) converged = true;
else if ( !is_trivial( norm ) && rel_converged( res, norm ) ) converged = true;
if ( isOK( converged ) ) return true;
for ( int i = 0; i < A_.m(); ++i )
{
Coef a = 1. / A_( i, i );
r_( i ) = a * r_( i ) + x( i );
}
ELAI_SYNC( r_ );
x = r_;
r_ = b_ - A_ * x;
res = sync_norm( r_ );
}
return false;
}
void eqnsys<nr_type_t>::factorize_qr_householder (void) {
int c, r, pivot;
nr_double_t s, MaxPivot;
delete T; T = new tvector<nr_type_t> (N);
for (c = 0; c < N; c++) {
// compute column norms and save in work array
nPvt[c] = euclidian_c (c);
cMap[c] = c; // initialize permutation vector
}
for (c = 0; c < N; c++) {
// put column with largest norm into pivot position
MaxPivot = nPvt[c]; pivot = c;
for (r = c + 1; r < N; r++)
if ((s = nPvt[r]) > MaxPivot) {
pivot = r;
MaxPivot = s;
}
if (pivot != c) {
A->exchangeCols (pivot, c);
Swap (int, cMap[pivot], cMap[c]);
Swap (nr_double_t, nPvt[pivot], nPvt[c]);
}
// compute and apply householder vector
T_(c) = householder_left (c);
// update norms of remaining columns too
for (r = c + 1; r < N; r++) {
if ((s = nPvt[r]) > 0) {
nr_double_t y = 0;
nr_double_t t = norm (A_(c, r) / s);
if (t < 1)
y = s * sqrt (1 - t);
if (fabs (y / s) < NR_TINY)
nPvt[r] = euclidian_c (r, c + 1);
else
nPvt[r] = y;
}
}
}
}
void eqnsys<nr_type_t>::factorize_svd (void) {
int i, j, l;
nr_type_t t;
// allocate space for vectors and matrices
delete R; R = new tvector<nr_type_t> (N);
delete T; T = new tvector<nr_type_t> (N);
delete V; V = new tmatrix<nr_type_t> (N);
delete S; S = new tvector<nr_double_t> (N);
delete E; E = new tvector<nr_double_t> (N);
// bidiagonalization through householder transformations
for (i = 0; i < N; i++) {
T_(i) = householder_left (i);
if (i < N - 1) R_(i) = householder_right (i);
}
// copy over the real valued bidiagonal values
for (i = 0; i < N; i++) S_(i) = real (A_(i, i));
for (E_(0) = 0, i = 1; i < N; i++) E_(i) = real (A_(i - 1, i));
// backward accumulation of right-hand householder transformations
// yields the V' matrix
for (l = N, i = N - 1; i >= 0; l = i--) {
if (i < N - 1) {
if ((t = R_(i)) != 0.0) {
householder_apply_right_extern (i, cond_conj (t));
}
else for (j = l; j < N; j++) // cleanup this row
V_(i, j) = V_(j, i) = 0.0;
}
V_(i, i) = 1.0;
}
// backward accumulation of left-hand householder transformations
// yields the U matrix in place of the A matrix
for (l = N, i = N - 1; i >= 0; l = i--) {
for (j = l; j < N; j++) // cleanup upper row
A_(i, j) = 0.0;
if ((t = T_(i)) != 0.0) {
householder_apply_left (i, cond_conj (t));
for (j = l; j < N; j++) A_(j, i) *= -t;
}
else for (j = l; j < N; j++) // cleanup this column
A_(j, i) = 0.0;
A_(i, i) = 1.0 - t;
}
// S and E contain diagonal and super-diagonal, A contains U, V'
// calculated; now diagonalization can begin
diagonalize_svd ();
}
void eqnsys<nr_type_t>::householder_apply_left (int c, nr_type_t t) {
nr_type_t f;
int k, r;
// apply the householder vector to each right-hand column
for (r = c + 1; r < N; r++) {
// calculate f = u' * A (a scalar product)
f = A_(c, r);
for (k = c + 1; k < N; k++) f += cond_conj (A_(k, c)) * A_(k, r);
// calculate A -= T * f * u
f *= cond_conj (t); A_(c, r) -= f;
for (k = c + 1; k < N; k++) A_(k, r) -= f * A_(k, c);
}
}
T eigen_power_iteration( const matrix<std::complex<T>,D,_A_>& A, O output, const T eps = T(1.0e-5) )
{
assert( A.row() == A.col() );
matrix<std::complex<T>,D,_A_> b( A.col(), 1 );
matrix<std::complex<T>,D,_A_> b_( A.col(), 1 );
std::copy( A.diag_cbegin(), A.diag_cend(), b.begin() ); // random initialize
matrix<std::complex<T>, D, _A_> A_(A);
std::for_each( A_.begin(), A_.end(), [](std::complex<T>& c) { c = std::conj(c); } );
for (;;)
{
auto const old_b = b;
b = A_*b;
std::transform( b.begin(), b.end(), b_.begin(), [](std::complex<T> v) { return std::conj(v); } );
auto const u_ = std::inner_product(b.begin(), b.end(), b_.begin(), std::complex<T>(0,0));
auto const u = real(u_);
auto const norm = std::sqrt(u);
b /= norm;
std::transform( b.begin(), b.end(), b_.begin(), [](std::complex<T> v) { return std::conj(v); } );
auto const U_ = std::inner_product(b.begin(), b.end(), b_.begin(), std::complex<T>(0,0));
auto const U = real(U_);
std::transform( old_b.begin(), old_b.end(), b_.begin(), [](std::complex<T> v) { return std::conj(v); } );
auto const V_ = std::inner_product(old_b.begin(), old_b.end(), b.begin(), std::complex<T>(0,0));
auto const V = real(V_);
auto const UV_ = std::inner_product(b.begin(), b.end(), b_.begin(), std::complex<T>(0,0));
auto const UV = real(UV_);
if ( UV * UV > U*V*(T(1)-eps) )
{
std::copy( b.begin(), b.end(), output );
return norm;
}
}
assert( !"eigen_power_iteration:: should never reach here!" );
return T(0); //just to kill warnings
}
