double ConvexHullProjectionConstraint::computeMargin(const Vector2& posIn2D)
{
bool isInside = true;
// First check if the point is inside the convex hull or not
iDynTree::VectorDynSize Ax(A.rows());
toEigen(Ax) = toEigen(A)*toEigen(posIn2D);
// If even one of the constraint is violated, then the point is outside the convex hull
for (int i=0; i < Ax.size(); i++)
{
if (!(Ax(i) <= b(i)))
{
isInside = false;
break;
}
}
// Then, compute the distance between each segment of the convex hull and the point :
// the minimum one is the distance of the point from the convex hull
// We start from the last segment that do not follow the segment[i],segment[i+1] structure
double distanceWithoutSign = distanceBetweenPointAndSegment(posIn2D,
projectedConvexHull(projectedConvexHull.getNrOfVertices()-1),
projectedConvexHull(0));
// Find the minimum distance
for (int i=0; i < projectedConvexHull.getNrOfVertices()-1; i++)
{
double candidateDistance = distanceBetweenPointAndSegment(posIn2D,
projectedConvexHull(i),
projectedConvexHull(i+1));
if (candidateDistance < distanceWithoutSign)
{
distanceWithoutSign = candidateDistance;
}
}
double margin;
// If the point is inside the convex hull, we return the distance, otherwise the negated distance
if (isInside)
{
margin = distanceWithoutSign;
}
else
{
margin = -distanceWithoutSign;
}
return margin;
}
// Constructors
F05_schwefel_global_opt_bound::F05_schwefel_global_opt_bound (int dimension, double bias) {
m_dimension = dimension;
m_bias = bias;
m_func_name = DEFAULT_FILE_DATA5;
// Note: dimension starts from 0
m_o = new double[m_dimension];
m_A = new double*[m_dimension];
for(int i=0;i<m_dimension;++i)
m_A[i]=new double[m_dimension];
m_B = new double[m_dimension];
m_z = new double[m_dimension];
double **m_data = new double*[m_dimension+1];
for(int i=0;i<m_dimension+1;++i)
m_data[i]=new double[m_dimension];
// Load the shifted global optimum
loadMatrixFromFile(DEFAULT_FILE_DATA5, m_dimension+1, m_dimension, m_data);
for (int i = 0 ; i < m_dimension ; i ++) {
if ((i+1) <= ceil(m_dimension / 4.0))
m_o[i] = -100.0;
else if ((i+1) >= floor((3.0 * m_dimension) / 4.0))
m_o[i] = 100.0;
else
m_o[i] = m_data[0][i];
}
for (int i = 0 ; i < m_dimension ; i ++) {
for (int j = 0 ; j < m_dimension ; j ++) {
m_A[i][j] = m_data[i+1][j];
}
}
Ax(m_B, m_A, m_o,m_dimension);
}
static void test_mgcr(ITER *ip, int i, MAT *Q, MAT *R)
#endif
{
VEC vt, vt1;
static MAT *R1 = MNULL;
static VEC *r = VNULL, *r1 = VNULL;
VEC *rr;
int k, j;
Real sm;
/* check Q*Q^T = I */
vt.dim = vt.max_dim = ip->b->dim;
vt1.dim = vt1.max_dim = ip->b->dim;
Q = m_resize(Q, i + 1, ip->b->dim);
R1 = m_resize(R1, i + 1, i + 1);
r = v_resize(r, ip->b->dim);
r1 = v_resize(r1, ip->b->dim);
MEM_STAT_REG(R1, TYPE_MAT);
MEM_STAT_REG(r, TYPE_VEC);
MEM_STAT_REG(r1, TYPE_VEC);
m_zero(R1);
for (k = 1; k <= i; k++)
for (j = 1; j <= i; j++) {
vt.ve = Q->me[k];
vt1.ve = Q->me[j];
R1->me[k][j] = in_prod(&vt, &vt1);
}
for (j = 1; j <= i; j++)
R1->me[j][j] -= 1.0;
#ifndef MEX
if (m_norm_inf(R1) > MACHEPS * ip->b->dim)
printf(" ! (mgcr:) m_norm_inf(Q*Q^T) = %g\n", m_norm_inf(R1));
#endif
/* check (r_i,Ap_j) = 0 for j <= i */
ip->Ax(ip->A_par, ip->x, r);
v_sub(ip->b, r, r);
rr = r;
if (ip->Bx) {
ip->Bx(ip->B_par, r, r1);
rr = r1;
}
#ifndef MEX
printf(" ||r|| = %g\n", v_norm2(rr));
#endif
sm = 0.0;
for (j = 1; j <= i; j++) {
vt.ve = Q->me[j];
sm = max(sm, in_prod(&vt,rr));
}
#ifndef MEX
if (sm >= MACHEPS * ip->b->dim)
printf(" ! (mgcr:) max_j (r,Ap_j) = %g\n", sm);
#endif
}
static void test_gmres(ITER *ip, int i, MAT *Q, MAT *R,
VEC *givc, VEC *givs, double h_val)
#endif
{
VEC vt, vt1;
STATIC MAT *Q1=MNULL, *R1=MNULL;
int j;
/* test Q*A*Q^T = R */
Q = m_resize(Q,i+1,ip->b->dim);
Q1 = m_resize(Q1,i+1,ip->b->dim);
R1 = m_resize(R1,i+1,i+1);
MEM_STAT_REG(Q1,TYPE_MAT);
MEM_STAT_REG(R1,TYPE_MAT);
vt.dim = vt.max_dim = ip->b->dim;
vt1.dim = vt1.max_dim = ip->b->dim;
for (j=0; j <= i; j++) {
vt.ve = Q->me[j];
vt1.ve = Q1->me[j];
ip->Ax(ip->A_par,&vt,&vt1);
}
mmtr_mlt(Q,Q1,R1);
R1 = m_resize(R1,i+2,i+1);
for (j=0; j < i; j++)
R1->me[i+1][j] = 0.0;
R1->me[i+1][i] = h_val;
for (j = 0; j <= i; j++) {
rot_rows(R1,j,j+1,givc->ve[j],givs->ve[j],R1);
}
R1 = m_resize(R1,i+1,i+1);
m_sub(R,R1,R1);
/* if (m_norm_inf(R1) > MACHEPS*ip->b->dim) */
#ifndef MEX
printf(" %d. ||Q*A*Q^T - H|| = %g [cf. MACHEPS = %g]\n",
ip->steps,m_norm_inf(R1),MACHEPS);
#endif
/* check Q*Q^T = I */
Q = m_resize(Q,i+1,ip->b->dim);
mmtr_mlt(Q,Q,R1);
for (j=0; j <= i; j++)
R1->me[j][j] -= 1.0;
#ifndef MEX
if (m_norm_inf(R1) > MACHEPS*ip->b->dim)
printf(" ! m_norm_inf(Q*Q^T) = %g\n",m_norm_inf(R1));
#endif
#ifdef THREADSAFE
M_FREE(Q1);
M_FREE(R1);
#endif
}
void
TimeIterationPolicyLinear<mesh_Type, AssemblyPolicy, SolverPolicy>::
iterate ( vectorPtr_Type solution,
bcContainerPtr_Type bchandler,
const Real& currentTime )
{
Real rhsIterNorm ( 0.0 );
//
// STEP 1: Updating the system
//
displayer().leaderPrint ( "Updating the system... " );
*M_rhs = 0.0;
M_systemMatrix.reset ( new matrix_Type ( *M_solutionMap ) );
AssemblyPolicy::assembleSystem ( M_systemMatrix, M_rhs, solution, SolverPolicy::preconditioner() );
displayer().leaderPrint ( "done\n" );
//
// STEP 2: Applying the boundary conditions
//
displayer().leaderPrint ( "Applying BC... " );
bcManage ( *M_systemMatrix, *M_rhs, *uFESpace()->mesh(), uFESpace()->dof(), *bchandler, uFESpace()->feBd(), 1.0, currentTime );
M_systemMatrix->globalAssemble();
displayer().leaderPrint ( "done\n" );
// Extra information if we want to know the exact residual
if ( M_computeResidual )
{
rhsIterNorm = M_rhs->norm2();
}
//
// STEP 3: Solving the system
//
displayer().leaderPrint ( "Solving the system... \n" );
SolverPolicy::solve ( M_systemMatrix, M_rhs, solution );
if ( M_computeResidual )
{
vector_Type Ax ( solution->map() );
vector_Type res ( *M_rhs );
M_systemMatrix->matrixPtr()->Apply ( solution->epetraVector(), Ax.epetraVector() );
res.epetraVector().Update ( -1, Ax.epetraVector(), 1 );
Real residual;
res.norm2 ( &residual );
residual /= rhsIterNorm;
displayer().leaderPrint ( "Scaled residual: ", residual, "\n" );
}
}
Real
SolverAztecOO::computeResidual ( vector_type& solution, vector_type& rhs )
{
vector_type Ax ( solution.map() );
vector_type res ( rhs );
M_solver.GetUserMatrix()->Apply ( solution.epetraVector(), Ax.epetraVector() );
res.epetraVector().Update ( 1, Ax.epetraVector(), -1 );
Real residual;
res.norm2 ( &residual );
return residual;
}
// ============================================================================
double
ComputeNorm(const Epetra_RowMatrix* A, const Epetra_MultiVector* LHS,
const Epetra_MultiVector* RHS)
{
double TotalNorm = 0.0;
Epetra_MultiVector Ax(*RHS);
A->Multiply(false, *LHS, Ax);
Ax.Update(1.0, *RHS, -1.0);
vector<double> norm(LHS->NumVectors());
Ax.Norm2(&norm[0]);
for (int i = 0 ; i < LHS->NumVectors() ; ++i)
TotalNorm += norm[i];
return(TotalNorm);
}
// Function body
double F05_schwefel_global_opt_bound::f(double *x,int length) {
/*double temp1 = pow((x[0]-x[2]), 2);
double temp2 = pow((x[2]-x[3]), 2);
double temp3 = pow((x[1]-x[4]), 2);
double temp5 = temp1 + temp2 + temp3;
return temp5;*/
double max = -HUGE_VAL; //negative inf
Ax(m_z, m_A, x,length);
for (int i = 0 ; i < m_dimension ; i ++) {
double temp = fabs(m_z[i] - m_B[i]);
if (max < temp)
max = temp;
}
return (max + m_bias);
}
Foam::coupledSolverPerformance Foam::coupledSmoothSolver::solve
(
FieldField<Field, scalar>& x,
const FieldField<Field, scalar>& b,
const direction cmpt
) const
{
// Prepare solver performance
coupledSolverPerformance solverPerf(typeName, fieldName());
// Do a minimum number of sweeps
// HJ, 19/Jan/2009
if (minIter() > 0)
{
autoPtr<coupledLduSmoother> smootherPtr = coupledLduSmoother::New
(
matrix_,
bouCoeffs_,
intCoeffs_,
interfaces_,
dict()
);
smootherPtr->smooth
(
x,
b,
cmpt,
minIter()
);
solverPerf.nIterations() += minIter();
}
// Now do normal sweeps. HJ, 19/Jan/2009
FieldField<Field, scalar> Ax(x.size());
FieldField<Field, scalar> temp(x.size());
forAll (x, rowI)
{
Ax.set(rowI, new scalarField(x[rowI].size(), 0));
temp.set(rowI, new scalarField(x[rowI].size(), 0));
}
NaGePoint3D NaGePipeSurface::PointAtPara(const double uPar, const double vPar)
{
NaGePoint3D P;
NaGePoint3D O = baseCurve->PointAtPara(uPar);
NaGePoint3D D;
NaGeVector3D Dir;
if(uPar == 1)
{
D = baseCurve->PointAtPara(uPar-0.001);
Dir = NaGeVector3D(O, D);
}
else
{
D = baseCurve->PointAtPara(uPar+0.001);
Dir = NaGeVector3D(D, O);
}
NaGePoint3D fp = baseCurve->PointAtPara(baseCurve->FirstParameter());
NaGePoint3D dfp = baseCurve->PointAtPara(baseCurve->FirstParameter()+0.001);
NaGeVector3D Z(fp, dfp);
NaGeAxisSystem Ax(O, Z);
if(circular)
{
NaGeCircle3d C(Ax, itsRadius);
if(uPar == baseCurve->FirstParameter())
C.Reverse();
P = C.PointAtPara(vPar);
}
else
{
NaGeEllipse3d C(Ax, itsMajorRadius, itsRadius);
if(uPar == baseCurve->FirstParameter())
C.Reverse();
P = C.PointAtPara(vPar);
}
NaGeVector3D Ref = Dir^Z;
double ang = Dir.Angle(Z, Ref);
P.Translate(Ax.GetPosition(), O);
P.Rotate(NaGeOneAxis(O, Ref), -ang);
return P;
}
double GradientProjection::computeCost(
valarray<double> const &b,
valarray<double> const &x) const {
// computes cost = 2 b x - x A x
double cost = 2. * dotProd(b,x);
valarray<double> Ax(x.size());
for (unsigned i=0; i<denseSize; i++) {
Ax[i] = 0;
for (unsigned j=0; j<denseSize; j++) {
Ax[i] += (*denseQ)[i*denseSize+j]*x[j];
}
}
if(sparseQ) {
valarray<double> r(x.size());
sparseQ->rightMultiply(x,r);
Ax+=r;
}
return cost - dotProd(x,Ax);
}
int ML_Amesos_Solve( void *data, double x[], double rhs[] )
{
Amesos_Handle_Type *Amesos_Handle = (Amesos_Handle_Type *) data;
if (Amesos_Handle->A_Base == 0) return 0;
Amesos_BaseSolver *A_Base = (Amesos_BaseSolver *) Amesos_Handle->A_Base ;
Epetra_Time Time(A_Base->Comm());
Epetra_LinearProblem *Amesos_LinearProblem = (Epetra_LinearProblem *)A_Base->GetProblem() ;
const Epetra_BlockMap & map = Amesos_LinearProblem->GetOperator()->OperatorDomainMap() ;
Epetra_Vector EV_rhs( View, map, rhs ) ;
Epetra_Vector EV_lhs( View, map, x ) ;
Amesos_LinearProblem->SetRHS( &EV_rhs ) ;
Amesos_LinearProblem->SetLHS( &EV_lhs ) ;
A_Base->Solve() ;
TimeForSolve__ += Time.ElapsedTime();
NumSolves__++;
#ifdef ML_AMESOS_DEBUG
// verify that the residual is actually small (and print the max
// in the destruction phase)
Epetra_Vector Ax(map);
(Amesos_LinearProblem->GetMatrix())->Multiply(false,EV_lhs,Ax);
ML_CHK_ERR(Ax.Update(1.0, EV_rhs, -1.0));
double residual;
ML_CHK_ERR(Ax.Norm2(&residual));
if( residual > MaxError__ ) MaxError__ = residual;
#endif
return 0;
} //ML_Amesos_Solve()
Real
LinearSolver::computeResidual ( vectorPtr_Type solutionPtr )
{
if ( !M_operator || !M_rhs )
{
M_displayer->leaderPrint ( "SLV- WARNING: LinearSolver can not compute the residual if the operator and the RHS are not set!\n" );
return -1;
}
vector_Type Ax ( solutionPtr->map() );
vector_Type residual ( *M_rhs );
M_operator->Apply ( solutionPtr->epetraVector(), Ax.epetraVector() );
residual.epetraVector().Update ( 1, Ax.epetraVector(), -1 );
Real residualNorm;
residual.norm2 ( &residualNorm );
return residualNorm;
}
/* Loads an binary STL file by filename
* Returns 0 on success and -1 on failure */
int Shape::loadBinarySTL(string filename) {
// if(getFileType(filename) != BINARY_STL) {
// return -1;
// }
triangles.clear();
Min.x = Min.y = Min.z = numeric_limits<double>::infinity();
Max.x = Max.y = Max.z = -numeric_limits<double>::infinity();
ifstream file;
file.open(filename.c_str());
if(file.fail()) {
cerr << _("Error: Unable to open stl file - ") << filename << endl;
return -1;
}
/* Binary STL files have a meaningless 80 byte header
* followed by the number of triangles */
file.seekg(80, ios_base::beg);
unsigned int num_triangles;
unsigned char buffer[4];
file.read(reinterpret_cast <char *> (buffer), 4);
// Read platform independent 32-bit little-endian int.
num_triangles = buffer[0] | buffer[1] << 8 | buffer[2] << 16 | buffer[3] << 24;
triangles.reserve(num_triangles);
for(uint i = 0; i < num_triangles; i++)
{
double a,b,c;
a = read_double (file);
b = read_double (file);
c = read_double (file);
Vector3d N(a,b,c);
a = read_double (file);
b = read_double (file);
c = read_double (file);
Vector3d Ax(a,b,c);
a = read_double (file);
b = read_double (file);
c = read_double (file);
Vector3d Bx(a,b,c);
a = read_double (file);
b = read_double (file);
c = read_double (file);
Vector3d Cx(a,b,c);
// done in Triangle
/* Recalculate normal vector - can't trust an STL file! */
// Vector3d AA=Cx-Ax;
// Vector3d BB=Cx-Bx;
// N = AA.cross(BB).getNormalized();
/* attribute byte count - sometimes contains face color
information but is useless for our purposes */
unsigned short byte_count;
file.read(reinterpret_cast <char *> (buffer), 2);
byte_count = buffer[0] | buffer[1] << 8;
// Repress unused variable warning.
(void)&byte_count;
Triangle T(Ax,Bx,Cx);
//cout << "bin triangle "<< N << ":\n\t" << Ax << "/\n\t"<<Bx << "/\n\t"<<Cx << endl;
triangles.push_back(T);
}
file.close();
CenterAroundXY();
scale_factor = 1.0;
scale_factor_x=scale_factor_y=scale_factor_z = 1.0;
double vol = volume();
if (vol < 0) {
invertNormals();
vol = -vol;
}
cout << _("Shape has volume ") << vol << " mm^3"<<endl;
return 0;
}
VEC *iter_cg1(ITER *ip)
#endif
{
STATIC VEC *r = VNULL, *p = VNULL, *q = VNULL, *z = VNULL;
Real alpha;
double inner,nres;
VEC *rr; /* rr == r or rr == z */
if (ip == INULL)
error(E_NULL,"iter_cg");
if (!ip->Ax || !ip->b)
error(E_NULL,"iter_cg");
if ( ip->x == ip->b )
error(E_INSITU,"iter_cg");
if (!ip->stop_crit)
error(E_NULL,"iter_cg");
if ( ip->eps <= 0.0 )
ip->eps = MACHEPS;
r = v_resize(r,ip->b->dim);
p = v_resize(p,ip->b->dim);
q = v_resize(q,ip->b->dim);
MEM_STAT_REG(r,TYPE_VEC);
MEM_STAT_REG(p,TYPE_VEC);
MEM_STAT_REG(q,TYPE_VEC);
if (ip->Bx != (Fun_Ax)NULL) {
z = v_resize(z,ip->b->dim);
MEM_STAT_REG(z,TYPE_VEC);
rr = z;
}
else rr = r;
if (ip->x != VNULL) {
if (ip->x->dim != ip->b->dim)
error(E_SIZES,"iter_cg");
ip->Ax(ip->A_par,ip->x,p); /* p = A*x */
v_sub(ip->b,p,r); /* r = b - A*x */
}
else { /* ip->x == 0 */
ip->x = v_get(ip->b->dim);
ip->shared_x = FALSE;
v_copy(ip->b,r);
}
if (ip->Bx) (ip->Bx)(ip->B_par,r,p);
else v_copy(r,p);
inner = in_prod(p,r);
nres = sqrt(fabs(inner));
if (ip->info) ip->info(ip,nres,r,p);
if ( nres == 0.0) return ip->x;
for ( ip->steps = 0; ip->steps <= ip->limit; ip->steps++ )
{
ip->Ax(ip->A_par,p,q);
inner = in_prod(q,p);
if (sqrt(fabs(inner)) <= MACHEPS*ip->init_res)
error(E_BREAKDOWN,"iter_cg1");
alpha = in_prod(p,r)/inner;
v_mltadd(ip->x,p,alpha,ip->x);
v_mltadd(r,q,-alpha,r);
rr = r;
if (ip->Bx) {
ip->Bx(ip->B_par,r,z);
rr = z;
}
nres = in_prod(r,rr);
if (nres < 0.0) {
warning(WARN_RES_LESS_0,"iter_cg");
break;
}
nres = sqrt(fabs(nres));
if (ip->info) ip->info(ip,nres,r,z);
if (ip->steps == 0) ip->init_res = nres;
if ( ip->stop_crit(ip,nres,r,z) ) break;
alpha = -in_prod(rr,q)/inner;
v_mltadd(rr,p,alpha,p);
}
#ifdef THREADSAFE
V_FREE(r); V_FREE(p); V_FREE(q); V_FREE(z);
#endif
return ip->x;
}
G(3), 0, 0, 0, 0, 0,
};
/*
* Fuer Elise, Beethoven
* (Excuse my non-existent musical skill, Mr. B ;-)
*/
static int tune2[96*6] = {
D(3), D(4), D(5), 0, 0, 0,
Cx(3), Cx(4), Cx(5), 0, 0, 0,
D(3), D(4), D(5), 0, 0, 0,
Cx(3), Cx(4), Cx(5), 0, 0, 0,
D(3), D(4), D(5), 0, 0, 0,
A(2), A(3), A(4), 0, 0, 0,
C(3), C(4), C(5), 0, 0, 0,
Ax(2), Ax(3), Ax(4), 0, 0, 0,
G(2), G(3), G(4), 0, 0, 0,
D(1), D(2), D(3), 0, 0, 0,
G(1), G(2), G(3), 0, 0, 0,
Ax(1), Ax(2), Ax(3), 0, 0, 0,
D(2), D(3), D(4), 0, 0, 0,
G(2), G(3), G(4), 0, 0, 0,
A(2), A(3), A(4), 0, 0, 0,
D(1), D(2), D(3), 0, 0, 0,
A(1), A(2), A(3), 0, 0, 0,
D(2), D(3), D(4), 0, 0, 0,
Fx(2), Fx(3), Fx(4), 0, 0, 0,
A(2), A(3), A(4), 0, 0, 0,
Ax(2), Ax(3), Ax(4), 0, 0, 0,
D(1), D(2), D(3), 0, 0, 0,
static real conjugate_gradient(Operator A, Operator precon, int n, real *x, real *rhs, real tol, int maxit, int *flag){
real *z, *r, *p, *q, res = 10*tol, alpha;
real rho = 1.0e20, rho_old = 1, res0, beta;
real* (*Ax)(Operator o, real *in, real *out) = A->Operator_apply;
real* (*Minvx)(Operator o, real *in, real *out) = precon->Operator_apply;
int iter = 0;
z = N_GNEW(n,real);
r = N_GNEW(n,real);
p = N_GNEW(n,real);
q = N_GNEW(n,real);
r = Ax(A, x, r);
r = vector_subtract_to(n, rhs, r);
res0 = res = sqrt(vector_product(n, r, r))/n;
#ifdef DEBUG_PRINT
if (Verbose && 0){
fprintf(stderr, " cg iter = %d, residual = %g\n", iter, res);
}
#endif
while ((iter++) < maxit && res > tol*res0){
z = Minvx(precon, r, z);
rho = vector_product(n, r, z);
if (iter > 1){
beta = rho/rho_old;
p = vector_saxpy(n, z, p, beta);
} else {
MEMCPY(p, z, sizeof(real)*n);
}
q = Ax(A, p, q);
alpha = rho/vector_product(n, p, q);
x = vector_saxpy2(n, x, p, alpha);
r = vector_saxpy2(n, r, q, -alpha);
res = sqrt(vector_product(n, r, r))/n;
#ifdef DEBUG_PRINT
if (Verbose && 0){
fprintf(stderr, " cg iter = %d, residual = %g\n", iter, res);
}
#endif
rho_old = rho;
}
FREE(z); FREE(r); FREE(p); FREE(q);
#ifdef DEBUG
_statistics[0] += iter - 1;
#endif
#ifdef DEBUG_PRINT
if (Verbose && 0){
fprintf(stderr, " cg iter = %d, residual = %g\n", iter, res);
}
#endif
return res;
}
int main(int argc, char* argv[]) {
int N;
scanf("%d", &N);
for (int s0 = 0; s0 < N; ++s0) {
int n;
scanf("%d", &n);
double c;
scanf("%lf", &c);
int maxit;
scanf("%d", &maxit);
double eps;
scanf("%lf", &eps);
SquareMatrix A(n);
for (int i = 1; i <= n; ++i) {
for (int j = 1; j <= n; ++j) {
double num;
scanf("%lf", &num);
A.setM(i, j, num);
}
}
Vector x(n);
for (int i = 1; i <= n; ++i) {
double num;
scanf("%lf", &num);
x.setV(i, num);
}
SquareMatrix A1(n);
A.copyTo(&A1);
for (int i = 1; i <= n; ++i) {
A1.setM(i, i, A.M(i, i) - c);
}
PMatrix P(n);
A1.PLUDecomposite(&P);
if (!A1.regular()) {
printf("%.8lf\n", c);
continue;
}
if (x.nullVector()) {
printf("kezdovektor\n", c);
continue;
}
x.multiplyScalar(1 / x.norm2());
Vector Ax(n);
x.leftMultiply(&A, &Ax);
double l0 = x.scalarProduct(&Ax);
double l1;
int m;
for (m = 1; m <= maxit; ++m) {
Vector y(n);
x.leftMultiply(&P, &y);
A1.solve(&y);
y.copyTo(&x);
x.multiplyScalar(1 / x.norm2());
x.leftMultiply(&A, &Ax);
l1 = x.scalarProduct(&Ax);
if (fabs(l1 - l0) < eps * (1 + fabs(l1))) break;
l0 = l1;
}
if (m > maxit) {
printf("maxit\n");
continue;
}
double d = 0;
for (int i = 1; i <= n; ++i) {
d += (Ax.V(i) - l1 * x.V(i)) * (Ax.V(i) - l1 * x.V(i));
}
printf(d <= eps ? "siker" : "sikertelen");
printf(" %.8lf", l1);
for (int i = 1; i <= n; ++i) {
printf(" %.8lf", x.V(i));
}
printf(" %.8lf", d);
printf("\n");
}
return 0;
}
void Model :: MD_3D(double le,int BstartID,int beforeN,int newN,double r,double region[3][2])
{
//分子動力学によりnewN個の粒子の位置を最適化 IDがBstartIDからBendIDまでのは境界粒子なので動かさない
double k0=1;
double dt=0.001;
int BendID=beforeN;
//力はax^3+bx^2+dの式を採用。文献[Bubble Mesh Automated Triangular Meshing of Non-Manifold Geometry by Sphere Packing]を参照
double a=(r+1)/(2*r*r-r-1)*k0/(le*le);
double b=-0.5*k0/le-1.5*a*le;
double d=-a*le*le*le-b*le*le;
/////////////
int lastN=beforeN+newN;
//cout<<"F="<<a*le*le*le+b*le*le+d<<" "<<a*1.5*le*1.5*le*1.5*le+b*1.5*le*1.5*le+d<<endl;
vector<double> Fx(newN); //各粒子に働くX方向力
vector<double> Fy(newN); //各粒子に働くY方向力
vector<double> Fz(newN); //各粒子に働くZ方向力
vector<double> Ax(newN,0); //X方向加速度
vector<double> Ay(newN,0); //Y方向加速度
vector<double> Az(newN,0); //Z方向加速度
vector<double> U(newN,0); //X方向速度
vector<double> V(newN,0); //Y方向速度
vector<double> W(newN,0); //Z方向速度
vector<double> visX(newN); //X方向粘性係数
vector<double> visY(newN); //Y方向粘性係数
vector<double> visZ(newN); //Y方向粘性係数
vector<double> KX(newN); //X方向バネ係数
vector<double> KY(newN); //Y方向バネ係数
vector<double> KZ(newN); //Y方向バネ係数
//計算の高速化のために格子を形成 解析幅がr*leで割り切れるとは限らないので、はみ出したところは切り捨て。なので各軸とも正の方向には余裕を持つこと
double grid_width=le*((int)(r+1)); //格子の幅。rを含む整数*le
int grid_sizeX=(int)((region[A_X][1]-region[A_X][0])/grid_width); //X方向の格子の個数
int grid_sizeY=(int)((region[A_Y][1]-region[A_Y][0])/grid_width);
int grid_sizeZ=(int)((region[A_Z][1]-region[A_Z][0])/grid_width);
int grid_SIZE=grid_sizeX*grid_sizeY*grid_sizeZ;
int plane_SIZE=grid_sizeX*grid_sizeY;
int *index=new int[newN]; //各内部粒子を含む格子番号
vector<int> *MESH=new vector<int>[grid_SIZE]; //各メッシュに格納される粒子ID格納
for(int i=BstartID;i<BendID;i++) //まずは境界粒子を格子に格納
{
int xn=(int)((PART[i].Get_X()-region[A_X][0])/grid_width);//X方向に何個目の格子か
int yn=(int)((PART[i].Get_Y()-region[A_Y][0])/grid_width);//Y方向に何個目の格子か
int zn=(int)((PART[i].Get_Z()-region[A_Z][0])/grid_width);//Z方向に何個目の格子か
int number=zn*grid_sizeX*grid_sizeY+yn*grid_sizeX+xn;//粒子iを含む格子の番号
MESH[number].push_back(i);
}
for(int k=0;k<newN;k++) //つぎに内部粒子を格納
{
int i=beforeN+k;
int xn=(int)((PART[i].Get_X()-region[A_X][0])/grid_width);//X方向に何個目の格子か
int yn=(int)((PART[i].Get_Y()-region[A_Y][0])/grid_width);//Y方向に何個目の格子か
int zn=(int)((PART[i].Get_Z()-region[A_Z][0])/grid_width);//Z方向に何個目の格子か
int number=zn*grid_sizeX*grid_sizeY+yn*grid_sizeX+xn;//粒子iを含む格子の番号
MESH[number].push_back(i);
index[k]=number;
}
//計算開始
for(int t=0;t<100;t++)
{
if(t%10==0 && t>0)
{
//MESHを一度破壊する。
for(int n=0;n<grid_SIZE;n++)
{
size_t size=MESH[n].size();
for(int k=0;k<size;k++) MESH[n].pop_back();
}
for(int i=BstartID;i<BendID;i++) //まずは境界粒子を格子に格納
{
int xn=(int)((PART[i].Get_X()-region[A_X][0])/grid_width);//X方向に何個目の格子か
int yn=(int)((PART[i].Get_Y()-region[A_Y][0])/grid_width);//Y方向に何個目の格子か
int zn=(int)((PART[i].Get_Z()-region[A_Z][0])/grid_width);//Z方向に何個目の格子か
int number=zn*grid_sizeX*grid_sizeY+yn*grid_sizeX+xn;//粒子iを含む格子の番号
MESH[number].push_back(i);
}
for(int k=0;k<newN;k++) //つぎに内部粒子を格納
{
int i=beforeN+k;
int xn=(int)((PART[i].Get_X()-region[A_X][0])/grid_width);//X方向に何個目の格子か
int yn=(int)((PART[i].Get_Y()-region[A_Y][0])/grid_width);//Y方向に何個目の格子か
int zn=(int)((PART[i].Get_Z()-region[A_Z][0])/grid_width);//Z方向に何個目の格子か
int number=zn*grid_sizeX*grid_sizeY+yn*grid_sizeX+xn;//粒子iを含む格子の番号
MESH[number].push_back(i);
index[k]=number;
}
}
for(int k=0;k<newN;k++)
{
Fx[k]=0; Fy[k]=0, Fz[k]=0; //初期化
KX[k]=0;KY[k]=0; KZ[k]=0; //バネ係数
}
for(int k=0;k<newN;k++)
{
int i=beforeN+k; //対応する粒子番号
int G_id=index[k]; //格納する格子番号
for(int II=G_id-1;II<=G_id+1;II++)
{
for(int JJ=-1*grid_sizeX;JJ<=grid_sizeX;JJ+=grid_sizeX)
{
for(int KK=-1*plane_SIZE;KK<=plane_SIZE;KK+=plane_SIZE)
{
int M_id=II+JJ+KK;
for(int L=0;L<MESH[M_id].size();L++)
{
int j=MESH[M_id][L];
if(j>=beforeN && j>i) //同じ領域内でかつiより大きな番号なら
{
int J=j-beforeN; //newN内での番号
double x=PART[j].Get_X()-PART[i].Get_X();
double y=PART[j].Get_Y()-PART[i].Get_Y();
double z=PART[j].Get_Z()-PART[i].Get_Z();
double dis=sqrt(x*x+y*y+z*z);
if(dis<r*le) //このloopは自分自身も通過するから、dis!=0は必要
{
double F=a*dis*dis*dis+b*dis*dis+d;
Fx[k]-=F*x/dis; //Fの値が正のときは斥力なので、-=にする
Fy[k]-=F*y/dis;
Fz[k]-=F*z/dis;
Fx[J]+=F*x/dis; //相手粒子の力もここで計算。符号は反転させる
Fy[J]+=F*y/dis;
Fz[J]+=F*z/dis;
double K=3*a*dis*dis+2*b*dis;//バネ係数 力の式の微分に相当
K=sqrt(K*K); //正の値が欲しい。だから負のときに備えて正に変換
KX[k]+=K*x*x/(dis*dis); //kを各方向に分配。ここで、常に正の量が分配されるようにx*x/(dis*dis)となっている
KY[k]+=K*y*y/(dis*dis);
KZ[k]+=K*z*z/(dis*dis);
KX[J]+=K*x*x/(dis*dis); //kを相手粒子にも分配
KY[J]+=K*y*y/(dis*dis);
KZ[J]+=K*z*z/(dis*dis);
}
}
if(j<BendID && j>=BstartID)
{
double x=PART[j].Get_X()-PART[i].Get_X();
double y=PART[j].Get_Y()-PART[i].Get_Y();
double z=PART[j].Get_Z()-PART[i].Get_Z();
double dis=sqrt(x*x+y*y+z*z);
if(dis<r*le && dis>0) //このloopは自分自身は通過しない、dis!=0は不要
{
double F=a*dis*dis*dis+b*dis*dis+d;
Fx[k]-=F*x/dis; //Fの値が正のときは斥力なので、-=にする
Fy[k]-=F*y/dis;
Fz[k]-=F*z/dis;
double K=3*a*dis*dis+2*b*dis;//バネ係数 力の式の微分に相当
K=sqrt(K*K); //正の値が欲しい。だから負のときに備えて正に変換
KX[k]+=K*x*x/(dis*dis); //kを各方向に分配。ここで、常に正の量が分配されるようにx*x/(dis*dis)となっている
KY[k]+=K*y*y/(dis*dis);
KZ[k]+=K*z*z/(dis*dis);
}
}
}
}
}
}
//visX[k]=1.414*sqrt(KX[k]);//このように各軸方向の粘性係数を決める。文献「物理モデルによる自動メッシュ分割」P6参照。ただし質量は1としている。
//visY[k]=1.414*sqrt(KY[k]);
//visZ[k]=1.414*sqrt(KZ[k]);
visX[k]=1.414*sqrt(KX[k]);//このように各軸方向の粘性係数を決める。文献「物理モデルによる自動メッシュ分割」P6参照。ただし質量は1としている。
visY[k]=1.414*sqrt(KY[k]);
visZ[k]=1.414*sqrt(KZ[k]);
Ax[k]=(Fx[k]-visX[k]*U[k]);
Ay[k]=(Fy[k]-visY[k]*V[k]);
Az[k]=(Fz[k]-visZ[k]*W[k]);
}//各粒子の加速度が求まった。
if(t==0) //最初のｽﾃｯﾌﾟ時にdtを決定
{
double MaxAccel=0;
for(int k=0;k<newN;k++)
{
double accel2=Ax[k]*Ax[k]+Ay[k]*Ay[k]+Az[k]*Az[k];
if(accel2>MaxAccel) MaxAccel=accel2;
}
MaxAccel=sqrt(MaxAccel);//最大加速度が求まった
if(MaxAccel!=0)
{
dt=sqrt(0.02*le/MaxAccel);
}
}
for(int k=0;k<newN;k++)//速度と位置の更新
{
int i=beforeN+k;
double u=U[k];
double v=V[k];
double w=W[k];
U[k]+=dt*Ax[k];
V[k]+=dt*Ay[k];
W[k]+=dt*Az[k];
PART[i].Add(dt*(U[k]+u)*0.5, dt*(V[k]+v)*0.5, dt*(W[k]+w)*0.5);
}
//再近接距離がle以下の場合はこれを修正
for(int k=0;k<newN;k++)
{
int i=beforeN+k; //対応する粒子番号
int G_id=index[k]; //格納する格子番号
double mindis=le;
int J=k; //最近接距離の相手粒子
for(int II=G_id-1;II<=G_id+1;II++)
{
for(int JJ=-1*grid_sizeX;JJ<=grid_sizeX;JJ+=grid_sizeX)
{
for(int KK=-1*plane_SIZE;KK<=plane_SIZE;KK+=plane_SIZE)
{
int M_id=II+JJ+KK;
for(int L=0;L<MESH[M_id].size();L++)
{
int j=MESH[M_id][L];
double x=PART[j].Get_X()-PART[i].Get_X();
double y=PART[j].Get_Y()-PART[i].Get_Y();
double z=PART[j].Get_Z()-PART[i].Get_Z();
double dis=sqrt(x*x+y*y+z*z);
if(dis<mindis && i!=j)
{
mindis=dis;
J=j;
}
}
}
}
}
if(J!=i && J<beforeN)//leより近接している相手が境界粒子なら
{
double L=le-mindis;//開くべき距離
double dX=PART[J].Get_X()-PART[i].Get_X();
double dY=PART[J].Get_Y()-PART[i].Get_Y();
double dZ=PART[J].Get_Z()-PART[i].Get_Z();
PART[i].Add(-dX/mindis*L, -dY/mindis*L, -dZ/mindis*L);
}
else if(J!=i && J>=beforeN)//leより近接している相手が内部粒子なら
{
double L=0.5*(le-mindis);//開くべき距離
double dX=PART[J].Get_X()-PART[i].Get_X();
double dY=PART[J].Get_Y()-PART[i].Get_Y();
double dZ=PART[J].Get_Z()-PART[i].Get_Z();
PART[i].Add(-dX/mindis*L, -dY/mindis*L, -dZ/mindis*L);
PART[J].Add(dX/mindis*L, dY/mindis*L, dZ/mindis*L);
}
}//////////*/
}/////MD終了
delete [] index;
delete [] MESH;
}
int
Stokhos::GMRESDivisionExpansionStrategy<ordinal_type,value_type,node_type>::
GMRES(const Teuchos::SerialDenseMatrix<int, double> & A, Teuchos::SerialDenseMatrix<int,double> & X, const Teuchos::SerialDenseMatrix<int,double> & B, int max_iter, double tolerance, int prec_iter, int order, int dim, int PrecNum, const Teuchos::SerialDenseMatrix<int, double> & M, int diag)
{
int n = A.numRows();
int k = 1;
double resid;
Teuchos::SerialDenseMatrix<int, double> P(n,n);
Teuchos::SerialDenseMatrix<int, double> Ax(n,1);
Ax.multiply(Teuchos::NO_TRANS,Teuchos::NO_TRANS,1.0, A, X, 0.0);
Teuchos::SerialDenseMatrix<int, double> r0(B);
r0-=Ax;
resid=r0.normFrobenius();
//define vector v=r/norm(r) where r=b-Ax
Teuchos::SerialDenseMatrix<int, double> v(n,1);
r0.scale(1/resid);
Teuchos::SerialDenseMatrix<int, double> h(1,1);
//Matrix of orthog basis vectors V
Teuchos::SerialDenseMatrix<int, double> V(n,1);
//Set v=r0/norm(r0) to be 1st col of V
for (int i=0; i<n; i++) {
V(i,0)=r0(i,0);
}
//right hand side
Teuchos::SerialDenseMatrix<int, double> bb(1,1);
bb(0,0)=resid;
Teuchos::SerialDenseMatrix<int, double> w(n,1);
Teuchos::SerialDenseMatrix<int, double> c;
Teuchos::SerialDenseMatrix<int, double> s;
while (resid > tolerance && k < max_iter) {
h.reshape(k+1,k);
//Arnoldi iteration(Gram-Schmidt )
V.reshape(n,k+1);
//set vk to be kth col of V
Teuchos::SerialDenseMatrix<int, double> vk(Teuchos::Copy, V, n,1,0,k-1);
//Preconditioning step: solve Mz=vk
Teuchos::SerialDenseMatrix<int, double> z(vk);
if (PrecNum == 1) {
Stokhos::DiagPreconditioner precond(M);
precond.ApplyInverse(vk,z,prec_iter);
}
else if (PrecNum == 2) {
Stokhos::JacobiPreconditioner precond(M);
precond.ApplyInverse(vk,z,2);
}
else if (PrecNum == 3) {
Stokhos::GSPreconditioner precond(M,1);
precond.ApplyInverse(vk,z,1);
}
else if (PrecNum == 4) {
Stokhos::SchurPreconditioner precond(M, order, dim, diag);
precond.ApplyInverse(vk,z,prec_iter);
}
w.multiply(Teuchos::NO_TRANS, Teuchos::NO_TRANS, 1, A, z, 0.0);
Teuchos::SerialDenseMatrix<int, double> vi(n,1);
Teuchos::SerialDenseMatrix<int, double> ip(1,1);
for (int i=0; i<k; i++) {
//set vi to be ith col of V
Teuchos::SerialDenseMatrix<int, double> vi(Teuchos::Copy, V, n,1,0,i);
//Calculate inner product
ip.multiply(Teuchos::TRANS, Teuchos::NO_TRANS, 1.0, vi, w, 0.0);
h(i,k-1)= ip(0,0);
//scale vi by h(i,k-1)
vi.scale(ip(0,0));
w-=vi;
}
h(k,k-1)=w.normFrobenius();
w.scale(1.0/h(k,k-1));
//add column vk+1=w to V
for (int i=0; i<n; i++) {
V(i,k)=w(i,0);
}
//Solve upper hessenberg least squares problem via Givens rotations
//Compute previous Givens rotations
for (int i=0; i<k-1; i++) {
double q=c(i,0)*h(i,k-1)+s(i,0)*h(i+1,k-1);
h(i+1,k-1)=-1*s(i,0)*h(i,k-1)+c(i,0)*h(i+1,k-1);
h(i,k-1)=q;
}
//Compute next Givens rotations
c.reshape(k,1);
s.reshape(k,1);
bb.reshape(k+1,1);
double l = sqrt(h(k-1,k-1)*h(k-1,k-1)+h(k,k-1)*h(k,k-1));
c(k-1,0)=h(k-1,k-1)/l;
s(k-1,0)=h(k,k-1)/l;
// Givens rotation on h and bb
h(k-1,k-1)=l;
h(k,k-1)=0;
bb(k,0)=-s(k-1,0)*bb(k-1,0);
bb(k-1,0)=c(k-1,0)*bb(k-1,0);
//Determine residual
resid = fabs(bb(k,0));
k++;
}
//Extract upper triangular square matrix
bb.reshape(h.numRows()-1 ,1);
//Solve linear system
int info;
Teuchos::LAPACK<int, double> lapack;
lapack.TRTRS('U', 'N', 'N', h.numRows()-1, 1, h.values(), h.stride(), bb.values(), bb.stride(),&info);
Teuchos::SerialDenseMatrix<int, double> ans(X);
V.reshape(n,k-1);
ans.multiply(Teuchos::NO_TRANS, Teuchos::NO_TRANS, 1.0, V, bb, 0.0);
if (PrecNum == 1) {
Stokhos::DiagPreconditioner precond(M);
precond.ApplyInverse(ans,ans,prec_iter);
}
else if (PrecNum == 2) {
Stokhos::JacobiPreconditioner precond(M);
precond.ApplyInverse(ans,ans,2);
}
else if (PrecNum == 3) {
Stokhos::GSPreconditioner precond(M,1);
precond.ApplyInverse(ans,ans,1);
}
else if (PrecNum == 4) {
Stokhos::SchurPreconditioner precond(M, order, dim, diag);
precond.ApplyInverse(ans,ans,prec_iter);
}
X+=ans;
std::cout << "iteration count= " << k-1 << std::endl;
return 0;
}
inline bool TRobustRegressionL1PD(
const MATRIX_TYPE& A,
const Eigen::Matrix<REAL, Eigen::Dynamic, 1>& y,
Eigen::Matrix<REAL, Eigen::Dynamic, 1>& xp,
REAL pdtol=1e-3, unsigned pdmaxiter=50)
{
typedef Eigen::Matrix<REAL, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Matrix;
typedef Eigen::Matrix<REAL, Eigen::Dynamic, 1> Vector;
const unsigned M = (unsigned)y.size();
const unsigned N = (unsigned)xp.size();
assert(A.rows() == M && A.cols() == N);
const REAL alpha(0.01);
const REAL beta(0.5);
const REAL mu(10);
Vector x(xp);
Vector Ax(A*x);
Vector tmpM1(y-Ax);
Vector tmpM2(-tmpM1);
Vector tmpM3(tmpM1.cwiseAbs()), tmpM4(M);
Vector u = (tmpM3*REAL(0.95)).array() + tmpM3.maxCoeff()*REAL(0.10);
Vector fu1 = tmpM2-u;
Vector fu2 = tmpM1-u;
Vector lamu1(M), lamu2(M);
for (unsigned i=0; i<M; ++i) {
lamu1(i) = -1.0/fu1(i);
lamu2(i) = -1.0/fu2(i);
}
const MATRIX_TYPE At(A.transpose());
Vector Atv(At*(lamu1-lamu2));
REAL AtvNormSq = Atv.squaredNorm();
Vector rdual((-lamu1-lamu2).array() + REAL(1));
REAL rdualNormSq = rdual.squaredNorm();
Vector w2(M), sig1(M), sig2(M), sigx(M), dx(N), up(N), Atdv(N);
Vector Axp(M), Atvp(M);
Vector &Adx(sigx), &du(w2), &w1p(dx);
Matrix H11p(N,N);
Vector &dlamu1(tmpM3), &dlamu2(tmpM4);
for (unsigned pditer=0; pditer<pdmaxiter; ++pditer) {
// surrogate duality gap
const REAL sdg(-(fu1.dot(lamu1) + fu2.dot(lamu2)));
if (sdg < pdtol)
break;
const REAL tau(mu*2*M/sdg);
const REAL inv_tau = REAL(-1)/tau;
tmpM1 = (-lamu1.cwiseProduct(fu1)).array() + inv_tau;
tmpM2 = (-lamu2.cwiseProduct(fu2)).array() + inv_tau;
const REAL resnorm = sqrt(AtvNormSq + rdualNormSq + tmpM1.squaredNorm() + tmpM2.squaredNorm());
for (unsigned i=0; i<M; ++i) {
REAL& tmpM3i = tmpM3(i);
tmpM3i = inv_tau/fu1(i);
REAL& tmpM4i = tmpM4(i);
tmpM4i = inv_tau/fu2(i);
w2(i) = tmpM3i + tmpM4i - REAL(1);
}
tmpM1 = lamu1.cwiseQuotient(fu1);
tmpM2 = lamu2.cwiseQuotient(fu2);
sig1 = -tmpM1 - tmpM2;
sig2 = tmpM1 - tmpM2;
sigx = sig1 - sig2.cwiseAbs2().cwiseQuotient(sig1);
H11p = At*(Eigen::DiagonalMatrix<REAL,Eigen::Dynamic>(sigx)*A);
w1p = At*(tmpM4 - tmpM3 - (sig2.cwiseQuotient(sig1).cwiseProduct(w2)));
// optimized solver as A is positive definite and symmetric
dx = H11p.ldlt().solve(w1p);
Adx = A*dx;
du = (w2 - sig2.cwiseProduct(Adx)).cwiseQuotient(sig1);
dlamu1 = -tmpM1.cwiseProduct(Adx-du) - lamu1 + tmpM3;
dlamu2 = tmpM2.cwiseProduct(Adx+du) - lamu2 + tmpM4;
Atdv = At*(dlamu1-dlamu2);
// make sure that the step is feasible: keeps lamu1,lamu2 > 0, fu1,fu2 < 0
REAL s(1);
for (unsigned i=0; i<M; ++i) {
REAL& dlamu1i = dlamu1(i);
if (dlamu1i < 0) {
const REAL tmp = -lamu1(i)/dlamu1i;
if (s > tmp)
s = tmp;
}
REAL& dlamu2i = dlamu2(i);
if (dlamu2i < 0) {
const REAL tmp = -lamu2(i)/dlamu2i;
if (s > tmp)
s = tmp;
}
}
for (unsigned i=0; i<M; ++i) {
REAL& Adxi = Adx(i);
REAL& dui = du(i);
REAL Adx_du = Adxi-dui;
if (Adx_du > 0) {
const REAL tmp = -fu1(i)/Adx_du;
if (s > tmp)
s = tmp;
}
Adx_du = -Adxi-dui;
if (Adx_du > 0) {
const REAL tmp = -fu2(i)/Adx_du;
if (s > tmp)
s = tmp;
}
}
s *= REAL(0.99);
// backtrack
lamu1 += s*dlamu1; lamu2 += s*dlamu2;
rdual = (-lamu1-lamu2).array() + REAL(1);
rdualNormSq = rdual.squaredNorm();
bool suffdec = false;
unsigned backiter = 0;
do {
xp = x + s*dx; up = u + s*du;
Axp = Ax + s*Adx; Atvp = Atv + s*Atdv;
fu1 = Axp - y - up; fu2 = -Axp + y - up;
AtvNormSq = Atvp.squaredNorm();
tmpM1 = (-lamu1.cwiseProduct(fu1)).array() + inv_tau;
tmpM2 = (-lamu2.cwiseProduct(fu2)).array() + inv_tau;
const REAL newresnorm = sqrt(AtvNormSq + rdualNormSq + tmpM1.squaredNorm() + tmpM2.squaredNorm());
suffdec = (newresnorm <= (REAL(1)-alpha*s)*resnorm);
s = beta*s;
if (++backiter > 32) {
//("error: stuck backtracking, returning last iterate"); // see Section 4 of notes for more information
xp.swap(x);
return false;
}
} while (!suffdec);
// next iteration
x.swap(xp); u.swap(up);
Ax.swap(Axp); Atv.swap(Atvp);
}
return true;
}
VEC *iter_lsqr(ITER *ip)
#endif
{
STATIC VEC *u = VNULL, *v = VNULL, *w = VNULL, *tmp = VNULL;
Real alpha, beta, phi, phi_bar;
Real rho, rho_bar, rho_max, theta, nres;
Real s, c; /* for Givens' rotations */
int m, n;
if ( ! ip || ! ip->b || !ip->Ax || !ip->ATx )
error(E_NULL,"iter_lsqr");
if ( ip->x == ip->b )
error(E_INSITU,"iter_lsqr");
if (!ip->stop_crit || !ip->x)
error(E_NULL,"iter_lsqr");
if ( ip->eps <= 0.0 ) ip->eps = MACHEPS;
m = ip->b->dim;
n = ip->x->dim;
u = v_resize(u,(unsigned int)m);
v = v_resize(v,(unsigned int)n);
w = v_resize(w,(unsigned int)n);
tmp = v_resize(tmp,(unsigned int)n);
MEM_STAT_REG(u,TYPE_VEC);
MEM_STAT_REG(v,TYPE_VEC);
MEM_STAT_REG(w,TYPE_VEC);
MEM_STAT_REG(tmp,TYPE_VEC);
if (ip->x != VNULL) {
ip->Ax(ip->A_par,ip->x,u); /* u = A*x */
v_sub(ip->b,u,u); /* u = b-A*x */
}
else { /* ip->x == 0 */
ip->x = v_get(ip->b->dim);
ip->shared_x = FALSE;
v_copy(ip->b,u); /* u = b */
}
beta = v_norm2(u);
if ( beta == 0.0 ) return ip->x;
sv_mlt(1.0/beta,u,u);
(ip->ATx)(ip->AT_par,u,v);
alpha = v_norm2(v);
if ( alpha == 0.0 ) return ip->x;
sv_mlt(1.0/alpha,v,v);
v_copy(v,w);
phi_bar = beta;
rho_bar = alpha;
rho_max = 1.0;
for (ip->steps = 0; ip->steps <= ip->limit; ip->steps++) {
tmp = v_resize(tmp,m);
(ip->Ax)(ip->A_par,v,tmp);
v_mltadd(tmp,u,-alpha,u);
beta = v_norm2(u);
sv_mlt(1.0/beta,u,u);
tmp = v_resize(tmp,n);
(ip->ATx)(ip->AT_par,u,tmp);
v_mltadd(tmp,v,-beta,v);
alpha = v_norm2(v);
sv_mlt(1.0/alpha,v,v);
rho = sqrt(rho_bar*rho_bar+beta*beta);
if ( rho > rho_max )
rho_max = rho;
c = rho_bar/rho;
s = beta/rho;
theta = s*alpha;
rho_bar = -c*alpha;
phi = c*phi_bar;
phi_bar = s*phi_bar;
/* update ip->x & w */
if ( rho == 0.0 )
error(E_BREAKDOWN,"iter_lsqr");
v_mltadd(ip->x,w,phi/rho,ip->x);
v_mltadd(v,w,-theta/rho,w);
nres = fabs(phi_bar*alpha*c)*rho_max;
if (ip->info) ip->info(ip,nres,w,VNULL);
if (ip->steps == 0) ip->init_res = nres;
if ( ip->stop_crit(ip,nres,w,VNULL) ) break;
}
#ifdef THREADSAFE
V_FREE(u);
V_FREE(v);
V_FREE(w);
V_FREE(tmp);
#endif
return ip->x;
}
void
TimeIterationPolicyNonlinear<mesh_Type, AssemblyPolicy, SolverPolicy>::
iterate ( vectorPtr_Type solution,
bcContainerPtr_Type bchandler,
const Real& currentTime )
{
int subiter = 0;
Real normRhs ( 0.0 );
Real nonLinearResidual ( 0.0 );
Real rhsIterNorm ( 0.0 );
do
{
//
// STEP 1: Updating the system
//
displayer().leaderPrint ( "Updating the system... " );
*M_rhs = 0.0;
M_systemMatrix.reset ( new matrix_Type ( *M_solutionMap ) );
AssemblyPolicy::assembleSystem ( M_systemMatrix, M_rhs, solution, SolverPolicy::preconditioner() );
displayer().leaderPrint ( "done\n" );
//
// STEP 2: Applying the boundary conditions
//
displayer().leaderPrint ( "Applying BC... " );
bcManage ( *M_systemMatrix, *M_rhs, *uFESpace()->mesh(), uFESpace()->dof(), *bchandler, uFESpace()->feBd(), 1.0, currentTime );
M_systemMatrix->globalAssemble();
displayer().leaderPrint ( "done\n" );
// Norm of the rhs needed for the nonlinear convergence test
if ( subiter == 0 )
{
normRhs = M_rhs->norm2();
}
//
// STEP 3: Computing the residual
//
// Computing the RHS as RHS=b-Ax_k
vector_Type Ax ( solution->map() );
M_systemMatrix->matrixPtr()->Apply ( solution->epetraVector(), Ax.epetraVector() );
Ax.epetraVector().Update (-1, M_rhs->epetraVector(), 1);
nonLinearResidual = Ax.norm2();
displayer().leaderPrint ( "Nonlinear residual : ", nonLinearResidual, "\n" );
displayer().leaderPrint ( "Nonlinear residual (scaled) : ", nonLinearResidual / normRhs, "\n" );
if ( nonLinearResidual > M_nonLinearTolerance * normRhs )
{
displayer().leaderPrint ( "---\nSubiteration [", ++subiter, "]\n" );
// Extra information if we want to know the exact residual
if ( M_computeResidual )
{
rhsIterNorm = M_rhs->norm2();
}
//
// Solving the system
//
displayer().leaderPrint ( "Solving the system... \n" );
*solution = 0.0;
SolverPolicy::solve ( M_systemMatrix, M_rhs, solution );
// int numIter = SolverPolicy::solve( M_systemMatrix, M_rhs, solution );
// numIterSum += numIter; //
if ( M_computeResidual )
{
vector_Type Ax ( solution->map() );
vector_Type res ( *M_rhs );
M_systemMatrix->matrixPtr()->Apply ( solution->epetraVector(), Ax.epetraVector() );
res.epetraVector().Update ( -1, Ax.epetraVector(), 1 );
Real residual;
res.norm2 ( &residual );
residual /= rhsIterNorm;
displayer().leaderPrint ( "Scaled residual: ", residual, "\n" );
}
}
}
while ( nonLinearResidual > M_nonLinearTolerance * normRhs );
displayer().leaderPrint ( "Nonlinear iterations : ", subiter, "\n" );
}
ordinal_type
Stokhos::CGDivisionExpansionStrategy<ordinal_type,value_type,node_type>::
CG(const Teuchos::SerialDenseMatrix<ordinal_type, value_type> & A,
Teuchos::SerialDenseMatrix<ordinal_type,value_type> & X,
const Teuchos::SerialDenseMatrix<ordinal_type,value_type> & B,
ordinal_type max_iter,
value_type tolerance,
ordinal_type prec_iter,
ordinal_type order ,
ordinal_type m,
ordinal_type PrecNum,
const Teuchos::SerialDenseMatrix<ordinal_type, value_type> & M,
ordinal_type diag)
{
ordinal_type n = A.numRows();
ordinal_type k=0;
value_type resid;
Teuchos::SerialDenseMatrix<ordinal_type, value_type> Ax(n,1);
Ax.multiply(Teuchos::NO_TRANS,Teuchos::NO_TRANS,1.0, A, X, 0.0);
Teuchos::SerialDenseMatrix<ordinal_type, value_type> r(Teuchos::Copy,B);
r-=Ax;
resid=r.normFrobenius();
Teuchos::SerialDenseMatrix<ordinal_type, value_type> p(r);
Teuchos::SerialDenseMatrix<ordinal_type, value_type> rho(1,1);
Teuchos::SerialDenseMatrix<ordinal_type, value_type> oldrho(1,1);
Teuchos::SerialDenseMatrix<ordinal_type, value_type> pAp(1,1);
Teuchos::SerialDenseMatrix<ordinal_type, value_type> Ap(n,1);
value_type b;
value_type a;
while (resid > tolerance && k < max_iter){
Teuchos::SerialDenseMatrix<ordinal_type, value_type> z(r);
//Solve Mz=r
if (PrecNum != 0){
if (PrecNum == 1){
Stokhos::DiagPreconditioner<ordinal_type, value_type> precond(M);
precond.ApplyInverse(r,z,prec_iter);
}
else if (PrecNum == 2){
Stokhos::JacobiPreconditioner<ordinal_type, value_type> precond(M);
precond.ApplyInverse(r,z,2);
}
else if (PrecNum == 3){
Stokhos::GSPreconditioner<ordinal_type, value_type> precond(M,0);
precond.ApplyInverse(r,z,1);
}
else if (PrecNum == 4){
Stokhos::SchurPreconditioner<ordinal_type, value_type> precond(M, order, m, diag);
precond.ApplyInverse(r,z,prec_iter);
}
}
rho.multiply(Teuchos::TRANS,Teuchos::NO_TRANS,1.0, r, z, 0.0);
if (k==0){
p.assign(z);
rho.multiply(Teuchos::TRANS, Teuchos::NO_TRANS, 1.0, r, z, 0.0);
}
else {
b=rho(0,0)/oldrho(0,0);
p.scale(b);
p+=z;
}
Ap.multiply(Teuchos::NO_TRANS,Teuchos::NO_TRANS,1.0, A, p, 0.0);
pAp.multiply(Teuchos::TRANS,Teuchos::NO_TRANS,1.0, p, Ap, 0.0);
a=rho(0,0)/pAp(0,0);
Teuchos::SerialDenseMatrix<ordinal_type, value_type> scalep(p);
scalep.scale(a);
X+=scalep;
Ap.scale(a);
r-=Ap;
oldrho.assign(rho);
resid=r.normFrobenius();
k++;
}
//std::cout << "iteration count " << k << std::endl;
return 0;
}
VEC *iter_cgne(ITER *ip)
#endif
{
STATIC VEC *r = VNULL, *p = VNULL, *q = VNULL, *z = VNULL;
Real alpha, beta, inner, old_inner, nres;
VEC *rr1; /* pointer only */
if (ip == INULL)
error(E_NULL,"iter_cgne");
if (!ip->Ax || ! ip->ATx || !ip->b)
error(E_NULL,"iter_cgne");
if ( ip->x == ip->b )
error(E_INSITU,"iter_cgne");
if (!ip->stop_crit)
error(E_NULL,"iter_cgne");
if ( ip->eps <= 0.0 ) ip->eps = MACHEPS;
r = v_resize(r,ip->b->dim);
p = v_resize(p,ip->b->dim);
q = v_resize(q,ip->b->dim);
MEM_STAT_REG(r,TYPE_VEC);
MEM_STAT_REG(p,TYPE_VEC);
MEM_STAT_REG(q,TYPE_VEC);
z = v_resize(z,ip->b->dim);
MEM_STAT_REG(z,TYPE_VEC);
if (ip->x) {
if (ip->x->dim != ip->b->dim)
error(E_SIZES,"iter_cgne");
ip->Ax(ip->A_par,ip->x,p); /* p = A*x */
v_sub(ip->b,p,z); /* z = b - A*x */
}
else { /* ip->x == 0 */
ip->x = v_get(ip->b->dim);
ip->shared_x = FALSE;
v_copy(ip->b,z);
}
rr1 = z;
if (ip->Bx) {
(ip->Bx)(ip->B_par,rr1,p);
rr1 = p;
}
(ip->ATx)(ip->AT_par,rr1,r); /* r = A^T*B*(b-A*x) */
old_inner = 0.0;
for ( ip->steps = 0; ip->steps <= ip->limit; ip->steps++ )
{
rr1 = r;
if ( ip->Bx ) {
(ip->Bx)(ip->B_par,r,z); /* rr = B*r */
rr1 = z;
}
inner = in_prod(r,rr1);
nres = sqrt(fabs(inner));
if (ip->info) ip->info(ip,nres,r,rr1);
if (ip->steps == 0) ip->init_res = nres;
if ( ip->stop_crit(ip,nres,r,rr1) ) break;
if ( ip->steps ) /* if ( ip->steps > 0 ) ... */
{
beta = inner/old_inner;
p = v_mltadd(rr1,p,beta,p);
}
else /* if ( ip->steps == 0 ) ... */
{
beta = 0.0;
p = v_copy(rr1,p);
old_inner = 0.0;
}
(ip->Ax)(ip->A_par,p,q); /* q = A*p */
if (ip->Bx) {
(ip->Bx)(ip->B_par,q,z);
(ip->ATx)(ip->AT_par,z,q);
rr1 = q; /* q = A^T*B*A*p */
}
else {
(ip->ATx)(ip->AT_par,q,z); /* z = A^T*A*p */
rr1 = z;
}
alpha = inner/in_prod(rr1,p);
v_mltadd(ip->x,p,alpha,ip->x);
v_mltadd(r,rr1,-alpha,r);
old_inner = inner;
}
#ifdef THREADSAFE
V_FREE(r);
V_FREE(p);
V_FREE(q);
V_FREE(z);
#endif
return ip->x;
}
// CG
int CG(const Teuchos::SerialDenseMatrix<int, double> & A, Teuchos::SerialDenseMatrix<int,double> X,const Teuchos::SerialDenseMatrix<int,double> & B, int max_iter, double tolerance, Stokhos::DiagPreconditioner<int,double> prec)
{
int n;
int k=0;
double resid;
n=A.numRows();
std::cout << "A= " << A << std::endl;
std::cout << "B= " << B << std::endl;
Teuchos::SerialDenseMatrix<int, double> Ax(n,1);
Ax.multiply(Teuchos::NO_TRANS,Teuchos::NO_TRANS,1.0, A, X, 0.0);
Teuchos::SerialDenseMatrix<int, double> r(B);
r-=Ax;
resid=r.normFrobenius();
Teuchos::SerialDenseMatrix<int, double> rho(1,1);
Teuchos::SerialDenseMatrix<int, double> oldrho(1,1);
Teuchos::SerialDenseMatrix<int, double> pAp(1,1);
Teuchos::SerialDenseMatrix<int, double> Ap(n,1);
double b;
double a;
Teuchos::SerialDenseMatrix<int, double> p(r);
while (resid > tolerance && k < max_iter){
Teuchos::SerialDenseMatrix<int, double> z(r);
//z=M-1r
// prec.ApplyInverse(r,z);
rho.multiply(Teuchos::TRANS,Teuchos::NO_TRANS,1.0, r, z, 0.0);
if (k==0){
p.assign(z);
rho.multiply(Teuchos::TRANS, Teuchos::NO_TRANS, 1.0, r, z, 0.0);
}
else {
b=rho(0,0)/oldrho(0,0);
p.scale(b);
p+=z;
}
Ap.multiply(Teuchos::NO_TRANS,Teuchos::NO_TRANS,1.0, A, p, 0.0);
pAp.multiply(Teuchos::TRANS,Teuchos::NO_TRANS,1.0, p, Ap, 0.0);
a=rho(0,0)/pAp(0,0);
Teuchos::SerialDenseMatrix<int, double> scalep(p);
scalep.scale(a);
X+=scalep;
Ap.scale(a);
r-=Ap;
oldrho.assign(rho);
resid=r.normFrobenius();
k++;
}
std::cout << "X= " << X << std::endl;
return 0;
}
VEC *iter_cgs(ITER *ip, VEC *r0)
#endif
{
STATIC VEC *p = VNULL, *q = VNULL, *r = VNULL, *u = VNULL;
STATIC VEC *v = VNULL, *z = VNULL;
VEC *tmp;
Real alpha, beta, nres, rho, old_rho, sigma, inner;
if (ip == INULL)
error(E_NULL,"iter_cgs");
if (!ip->Ax || !ip->b || !r0)
error(E_NULL,"iter_cgs");
if ( ip->x == ip->b )
error(E_INSITU,"iter_cgs");
if (!ip->stop_crit)
error(E_NULL,"iter_cgs");
if ( r0->dim != ip->b->dim )
error(E_SIZES,"iter_cgs");
if ( ip->eps <= 0.0 ) ip->eps = MACHEPS;
p = v_resize(p,ip->b->dim);
q = v_resize(q,ip->b->dim);
r = v_resize(r,ip->b->dim);
u = v_resize(u,ip->b->dim);
v = v_resize(v,ip->b->dim);
MEM_STAT_REG(p,TYPE_VEC);
MEM_STAT_REG(q,TYPE_VEC);
MEM_STAT_REG(r,TYPE_VEC);
MEM_STAT_REG(u,TYPE_VEC);
MEM_STAT_REG(v,TYPE_VEC);
if (ip->Bx) {
z = v_resize(z,ip->b->dim);
MEM_STAT_REG(z,TYPE_VEC);
}
if (ip->x != VNULL) {
if (ip->x->dim != ip->b->dim)
error(E_SIZES,"iter_cgs");
ip->Ax(ip->A_par,ip->x,v); /* v = A*x */
if (ip->Bx) {
v_sub(ip->b,v,v); /* v = b - A*x */
(ip->Bx)(ip->B_par,v,r); /* r = B*(b-A*x) */
}
else v_sub(ip->b,v,r); /* r = b-A*x */
}
else { /* ip->x == 0 */
ip->x = v_get(ip->b->dim); /* x == 0 */
ip->shared_x = FALSE;
if (ip->Bx) (ip->Bx)(ip->B_par,ip->b,r); /* r = B*b */
else v_copy(ip->b,r); /* r = b */
}
v_zero(p);
v_zero(q);
old_rho = 1.0;
for (ip->steps = 0; ip->steps <= ip->limit; ip->steps++) {
inner = in_prod(r,r);
nres = sqrt(fabs(inner));
if (ip->steps == 0) ip->init_res = nres;
if (ip->info) ip->info(ip,nres,r,VNULL);
if ( ip->stop_crit(ip,nres,r,VNULL) ) break;
rho = in_prod(r0,r);
if ( old_rho == 0.0 )
error(E_BREAKDOWN,"iter_cgs");
beta = rho/old_rho;
v_mltadd(r,q,beta,u);
v_mltadd(q,p,beta,v);
v_mltadd(u,v,beta,p);
(ip->Ax)(ip->A_par,p,q);
if (ip->Bx) {
(ip->Bx)(ip->B_par,q,z);
tmp = z;
}
else tmp = q;
sigma = in_prod(r0,tmp);
if ( sigma == 0.0 )
error(E_BREAKDOWN,"iter_cgs");
alpha = rho/sigma;
v_mltadd(u,tmp,-alpha,q);
v_add(u,q,v);
(ip->Ax)(ip->A_par,v,u);
if (ip->Bx) {
(ip->Bx)(ip->B_par,u,z);
tmp = z;
}
else tmp = u;
v_mltadd(r,tmp,-alpha,r);
v_mltadd(ip->x,v,alpha,ip->x);
old_rho = rho;
}
#ifdef THREADSAFE
V_FREE(p);
V_FREE(q);
V_FREE(r);
V_FREE(u);
V_FREE(v);
V_FREE(z);
#endif
return ip->x;
}
int Amesos_Scalapack::Solve() {
if( debug_ == 1 ) std::cout << "Entering `Solve()'" << std::endl;
NumSolve_++;
Epetra_MultiVector *vecX = Problem_->GetLHS() ;
Epetra_MultiVector *vecB = Problem_->GetRHS() ;
//
// Compute the number of right hands sides
// (and check that X and B have the same shape)
//
int nrhs;
if ( vecX == 0 ) {
nrhs = 0 ;
EPETRA_CHK_ERR( vecB != 0 ) ;
} else {
nrhs = vecX->NumVectors() ;
EPETRA_CHK_ERR( vecB->NumVectors() != nrhs ) ;
}
Epetra_MultiVector *ScalapackB =0;
Epetra_MultiVector *ScalapackX =0;
//
// Extract Scalapack versions of X and B
//
double *ScalapackXvalues ;
Epetra_RowMatrix *RowMatrixA = dynamic_cast<Epetra_RowMatrix *>(Problem_->GetOperator());
Time_->ResetStartTime(); // track time to broadcast vectors
//
// Copy B to the scalapack version of B
//
const Epetra_Map &OriginalMap = RowMatrixA->RowMatrixRowMap();
Epetra_MultiVector *ScalapackXextract = new Epetra_MultiVector( *VectorMap_, nrhs ) ;
Epetra_MultiVector *ScalapackBextract = new Epetra_MultiVector( *VectorMap_, nrhs ) ;
Epetra_Import ImportToScalapack( *VectorMap_, OriginalMap );
ScalapackBextract->Import( *vecB, ImportToScalapack, Insert ) ;
ScalapackB = ScalapackBextract ;
ScalapackX = ScalapackXextract ;
VecTime_ += Time_->ElapsedTime();
//
// Call SCALAPACKs PDGETRS to perform the solve
//
int DescX[10];
ScalapackX->Scale(1.0, *ScalapackB) ;
int ScalapackXlda ;
Time_->ResetStartTime(); // tract time to solve
//
// Setup DescX
//
if( nrhs > nb_ ) {
EPETRA_CHK_ERR( -2 );
}
int Ierr[1] ;
Ierr[0] = 0 ;
const int zero = 0 ;
const int one = 1 ;
if ( iam_ < nprow_ * npcol_ ) {
assert( ScalapackX->ExtractView( &ScalapackXvalues, &ScalapackXlda ) == 0 ) ;
if ( false ) std::cout << "Amesos_Scalapack.cpp: " << __LINE__ << " ScalapackXlda = " << ScalapackXlda
<< " lda_ = " << lda_
<< " nprow_ = " << nprow_
<< " npcol_ = " << npcol_
<< " myprow_ = " << myprow_
<< " mypcol_ = " << mypcol_
<< " iam_ = " << iam_ << std::endl ;
if ( TwoD_distribution_ ) assert( mypcol_ >0 || EPETRA_MAX(ScalapackXlda,1) == lda_ ) ;
DESCINIT_F77(DescX,
&NumGlobalElements_,
&nrhs,
&nb_,
&nb_,
&zero,
&zero,
&ictxt_,
&lda_,
Ierr ) ;
assert( Ierr[0] == 0 ) ;
//
// For the 1D data distribution, we factor the transposed
// matrix, hence we must invert the sense of the transposition
//
char trans = 'N';
if ( TwoD_distribution_ ) {
if ( UseTranspose() ) trans = 'T' ;
} else {
if ( ! UseTranspose() ) trans = 'T' ;
}
if ( nprow_ * npcol_ == 1 ) {
DGETRS_F77(&trans,
&NumGlobalElements_,
&nrhs,
&DenseA_[0],
&lda_,
&Ipiv_[0],
ScalapackXvalues,
&lda_,
Ierr ) ;
} else {
PDGETRS_F77(&trans,
&NumGlobalElements_,
&nrhs,
&DenseA_[0],
&one,
&one,
DescA_,
&Ipiv_[0],
ScalapackXvalues,
&one,
&one,
DescX,
Ierr ) ;
}
}
SolTime_ += Time_->ElapsedTime();
Time_->ResetStartTime(); // track time to broadcast vectors
//
// Copy X back to the original vector
//
Epetra_Import ImportFromScalapack( OriginalMap, *VectorMap_ );
vecX->Import( *ScalapackX, ImportFromScalapack, Insert ) ;
delete ScalapackBextract ;
delete ScalapackXextract ;
VecTime_ += Time_->ElapsedTime();
// All processes should return the same error code
if ( nprow_ * npcol_ < Comm().NumProc() )
Comm().Broadcast( Ierr, 1, 0 ) ;
// MS // compute vector norms
if( ComputeVectorNorms_ == true || verbose_ == 2 ) {
double NormLHS, NormRHS;
for( int i=0 ; i<nrhs ; ++i ) {
assert((*vecX)(i)->Norm2(&NormLHS)==0);
assert((*vecB)(i)->Norm2(&NormRHS)==0);
if( verbose_ && Comm().MyPID() == 0 ) {
std::cout << "Amesos_Scalapack : vector " << i << ", ||x|| = " << NormLHS
<< ", ||b|| = " << NormRHS << std::endl;
}
}
}
// MS // compute true residual
if( ComputeTrueResidual_ == true || verbose_ == 2 ) {
double Norm;
Epetra_MultiVector Ax(vecB->Map(),nrhs);
for( int i=0 ; i<nrhs ; ++i ) {
(Problem_->GetMatrix()->Multiply(UseTranspose(), *((*vecX)(i)), Ax));
(Ax.Update(1.0, *((*vecB)(i)), -1.0));
(Ax.Norm2(&Norm));
if( verbose_ && Comm().MyPID() == 0 ) {
std::cout << "Amesos_Scalapack : vector " << i << ", ||Ax - b|| = " << Norm << std::endl;
}
}
}
return Ierr[0];
}
VEC *iter_gmres(ITER *ip)
#endif
{
STATIC VEC *u=VNULL, *r=VNULL, *rhs = VNULL;
STATIC VEC *givs=VNULL, *givc=VNULL, *z = VNULL;
STATIC MAT *Q = MNULL, *R = MNULL;
VEC *rr, v, v1; /* additional pointers (not real vectors) */
int i,j, done;
Real nres;
/* Real last_h; */
if (ip == INULL)
error(E_NULL,"iter_gmres");
if ( ! ip->Ax || ! ip->b )
error(E_NULL,"iter_gmres");
if ( ! ip->stop_crit )
error(E_NULL,"iter_gmres");
if ( ip->k <= 0 )
error(E_BOUNDS,"iter_gmres");
if (ip->x != VNULL && ip->x->dim != ip->b->dim)
error(E_SIZES,"iter_gmres");
if (ip->eps <= 0.0) ip->eps = MACHEPS;
r = v_resize(r,ip->k+1);
u = v_resize(u,ip->b->dim);
rhs = v_resize(rhs,ip->k+1);
givs = v_resize(givs,ip->k); /* Givens rotations */
givc = v_resize(givc,ip->k);
MEM_STAT_REG(r,TYPE_VEC);
MEM_STAT_REG(u,TYPE_VEC);
MEM_STAT_REG(rhs,TYPE_VEC);
MEM_STAT_REG(givs,TYPE_VEC);
MEM_STAT_REG(givc,TYPE_VEC);
R = m_resize(R,ip->k+1,ip->k);
Q = m_resize(Q,ip->k,ip->b->dim);
MEM_STAT_REG(R,TYPE_MAT);
MEM_STAT_REG(Q,TYPE_MAT);
if (ip->x == VNULL) { /* ip->x == 0 */
ip->x = v_get(ip->b->dim);
ip->shared_x = FALSE;
}
v.dim = v.max_dim = ip->b->dim; /* v and v1 are pointers to rows */
v1.dim = v1.max_dim = ip->b->dim; /* of matrix Q */
if (ip->Bx != (Fun_Ax)NULL) { /* if precondition is defined */
z = v_resize(z,ip->b->dim);
MEM_STAT_REG(z,TYPE_VEC);
}
done = FALSE;
for (ip->steps = 0; ip->steps < ip->limit; ) {
/* restart */
ip->Ax(ip->A_par,ip->x,u); /* u = A*x */
v_sub(ip->b,u,u); /* u = b - A*x */
rr = u; /* rr is a pointer only */
if (ip->Bx) {
(ip->Bx)(ip->B_par,u,z); /* tmp = B*(b-A*x) */
rr = z;
}
nres = v_norm2(rr);
if (ip->steps == 0) {
if (ip->info) ip->info(ip,nres,VNULL,VNULL);
ip->init_res = nres;
}
if ( nres == 0.0 ) {
done = TRUE;
break;
}
v.ve = Q->me[0];
sv_mlt(1.0/nres,rr,&v);
v_zero(r);
v_zero(rhs);
rhs->ve[0] = nres;
for ( i = 0; i < ip->k && ip->steps < ip->limit; i++ ) {
ip->steps++;
v.ve = Q->me[i];
(ip->Ax)(ip->A_par,&v,u);
rr = u;
if (ip->Bx) {
(ip->Bx)(ip->B_par,u,z);
rr = z;
}
if (i < ip->k - 1) {
v1.ve = Q->me[i+1];
v_copy(rr,&v1);
for (j = 0; j <= i; j++) {
v.ve = Q->me[j];
/* r->ve[j] = in_prod(&v,rr); */
/* modified Gram-Schmidt algorithm */
r->ve[j] = in_prod(&v,&v1);
v_mltadd(&v1,&v,-r->ve[j],&v1);
}
r->ve[i+1] = nres = v_norm2(&v1);
if (nres <= MACHEPS*ip->init_res) {
for (j = 0; j < i; j++)
rot_vec(r,j,j+1,givc->ve[j],givs->ve[j],r);
set_col(R,i,r);
done = TRUE;
break;
}
sv_mlt(1.0/nres,&v1,&v1);
}
else { /* i == ip->k - 1 */
/* Q->me[ip->k] need not be computed */
for (j = 0; j <= i; j++) {
v.ve = Q->me[j];
r->ve[j] = in_prod(&v,rr);
}
nres = in_prod(rr,rr) - in_prod(r,r);
if (sqrt(fabs(nres)) <= MACHEPS*ip->init_res) {
for (j = 0; j < i; j++)
rot_vec(r,j,j+1,givc->ve[j],givs->ve[j],r);
set_col(R,i,r);
done = TRUE;
break;
}
if (nres < 0.0) { /* do restart */
i--;
ip->steps--;
break;
}
r->ve[i+1] = sqrt(nres);
}
/* QR update */
/* last_h = r->ve[i+1]; */ /* for test only */
for (j = 0; j < i; j++)
rot_vec(r,j,j+1,givc->ve[j],givs->ve[j],r);
givens(r->ve[i],r->ve[i+1],&givc->ve[i],&givs->ve[i]);
rot_vec(r,i,i+1,givc->ve[i],givs->ve[i],r);
rot_vec(rhs,i,i+1,givc->ve[i],givs->ve[i],rhs);
set_col(R,i,r);
nres = fabs((double) rhs->ve[i+1]);
if (ip->info) ip->info(ip,nres,VNULL,VNULL);
if ( ip->stop_crit(ip,nres,VNULL,VNULL) ) {
done = TRUE;
break;
}
}
/* use ixi submatrix of R */
if (i >= ip->k) i = ip->k - 1;
R = m_resize(R,i+1,i+1);
rhs = v_resize(rhs,i+1);
/* test only */
/* test_gmres(ip,i,Q,R,givc,givs,last_h); */
Usolve(R,rhs,rhs,0.0); /* solve a system: R*x = rhs */
/* new approximation */
for (j = 0; j <= i; j++) {
v.ve = Q->me[j];
v_mltadd(ip->x,&v,rhs->ve[j],ip->x);
}
if (done) break;
/* back to old dimensions */
rhs = v_resize(rhs,ip->k+1);
R = m_resize(R,ip->k+1,ip->k);
}
#ifdef THREADSAFE
V_FREE(u);
V_FREE(r);
V_FREE(rhs);
V_FREE(givs);
V_FREE(givc);
V_FREE(z);
M_FREE(Q);
M_FREE(R);
#endif
return ip->x;
}
void Model :: MD_2D(double le,int BstartID,int beforeN,int newN)
{
//分子動力学によりnewN個の粒子の位置を最適化 IDがBstartIDからBendIDまでのは境界粒子なので動かさない
double region[2][2]; //解析領域
/////////////////////解析領域の決定
region[A_X][0]=100; region[A_X][1]=-100;
region[A_Y][0]=100; region[A_Y][1]=-100;
for(int i=BstartID;i<beforeN;i++)
{
if(PART[i].Get_X()<region[A_X][0]) region[A_X][0]=PART[i].Get_X();
else if(PART[i].Get_X()>region[A_X][1]) region[A_X][1]=PART[i].Get_X();
if(PART[i].Get_Y()<region[A_Y][0]) region[A_Y][0]=PART[i].Get_Y();
else if(PART[i].Get_Y()>region[A_Y][1]) region[A_Y][1]=PART[i].Get_Y();
}
for(int D=0;D<2;D++)
{
region[D][0]-=5*le; //少し領域を広めにとる
region[D][1]+=5*le;
}//////////////////////////
//パラメータ
double k0=1;
double r=1.5;
double dt=0.001;
//力はax^3+bx^2+dの式を採用。文献[Bubble Mesh Automated Triangular Meshing of Non-Manifold Geometry by Sphere Packing]を参照
double a=(r+1)/(2*r*r-r-1)*k0/(le*le);
double b=-0.5*k0/le-1.5*a*le;
double d=-a*le*le*le-b*le*le;
/////////////
int lastN=beforeN+newN;
vector<double> Fx(newN); //各粒子に働くX方向力
vector<double> Fy(newN); //各粒子に働くY方向力
vector<double> Ax(newN,0); //X方向加速度
vector<double> Ay(newN,0); //Y方向加速度
vector<double> U(newN,0); //X方向速度
vector<double> V(newN,0); //Y方向速度
vector<double> visX(newN); //X方向粘性係数
vector<double> visY(newN); //Y方向粘性係数
//計算の高速化のために格子を形成 解析幅がr*leで割り切れるとは限らないので、はみ出したところは切り捨て。なので各軸とも正の方向には余裕を持つこと
double grid_width=le*((int)(r+1)); //格子の幅。rを含む整数*le
int grid_sizeX=(int)((region[A_X][1]-region[A_X][0])/grid_width); //X方向の格子の個数
int grid_sizeY=(int)((region[A_Y][1]-region[A_Y][0])/grid_width);
int plane_SIZE=grid_sizeX*grid_sizeY;
int *index=new int[newN]; //各内部粒子を含む格子番号
// cout<<"ここっぽい"<<endl;
// vector<int> *MESH=new vector<int>[plane_SIZE]; //各メッシュに格納される粒子ID格納
vector<vector<int> > MESH;
MESH.resize(plane_SIZE);
for(int i=BstartID;i<beforeN;i++) //まずは境界粒子を格子に格納
{
int xn=(int)((PART[i].Get_X()-region[A_X][0])/grid_width); //X方向に何個目の格子か
int yn=(int)((PART[i].Get_Y()-region[A_Y][0])/grid_width); //Y方向に何個目の格子か
int number=yn*grid_sizeX+xn; //粒子iを含む格子の番号
MESH[number].push_back(i);
}
for(int k=0;k<newN;k++) //つぎに内部粒子を格納
{
int i=beforeN+k;
int xn=(int)((PART[i].Get_X()-region[A_X][0])/grid_width);//X方向に何個目の格子か
int yn=(int)((PART[i].Get_Y()-region[A_Y][0])/grid_width);//Y方向に何個目の格子か
int number=yn*grid_sizeX+xn; //粒子iを含む格子の番号
MESH[number].push_back(i);
index[k]=number;
}//////////////////////////////////////////
//計算開始
for(int t=0;t<100;t++)
{
if(t%10==0 &&t>0)//MESHを作り直す
{
//まずはMESHを一度破壊する。
for(int n=0;n<plane_SIZE;n++)
{
size_t size=MESH[n].size();
for(int k=0;k<size;k++) MESH[n].pop_back();
}
for(int i=BstartID;i<beforeN;i++) //まずは境界粒子を格子に格納
{
int xn=(int)((PART[i].Get_X()-region[A_X][0])/grid_width); //X方向に何個目の格子か
int yn=(int)((PART[i].Get_Y()-region[A_Y][0])/grid_width); //Y方向に何個目の格子か
int number=yn*grid_sizeX+xn; //粒子iを含む格子の番号
MESH[number].push_back(i);
}
for(int k=0;k<newN;k++) //つぎに内部粒子を格納
{
int i=beforeN+k;
int xn=(int)((PART[i].Get_X()-region[A_X][0])/grid_width);//X方向に何個目の格子か
int yn=(int)((PART[i].Get_Y()-region[A_Y][0])/grid_width);//Y方向に何個目の格子か
int number=yn*grid_sizeX+xn; //粒子iを含む格子の番号
MESH[number].push_back(i);
index[k]=number;
}
}////////////
for(int k=0;k<newN;k++)
{
Fx[k]=0; Fy[k]=0; //初期化
int i=beforeN+k; //対応する粒子番号
double kx=0; //X方向バネ係数
double ky=0;
int G_id=index[k]; //格納する格子番号
for(int II=G_id-1;II<=G_id+1;II++)
{
for(int JJ=-1*grid_sizeX;JJ<=grid_sizeX;JJ+=grid_sizeX)
{
int M_id=II+JJ;
for(int L=0;L<MESH[M_id].size();L++)
{
int j=MESH[M_id][L];
double x=PART[j].Get_X()-PART[i].Get_X();
double y=PART[j].Get_Y()-PART[i].Get_Y();
double dis=sqrt(x*x+y*y);
if(dis<r*le && dis!=0) //このloopは自分自身も通過するから、dis!=0は必要
{
double F=a*dis*dis*dis+b*dis*dis+d;
Fx[k]-=F*x/dis; //Fの値が正のときは斥力なので、-=にする
Fy[k]-=F*y/dis;
double K=3*a*dis*dis+2*b*dis;//バネ係数 力の式の微分に相当
K=sqrt(K*K); //正の値が欲しい。だから負のときに備えて正に変換
kx+=K*x*x/(dis*dis); //kを各方向に分配。ここで、常に正の量が分配されるようにx*x/(dis*dis)となっている
ky+=K*y*y/(dis*dis);
}
}
}
}
visX[k]=1.414*sqrt(kx);//このように各軸方向の粘性係数を決める。文献「物理モデルによる自動メッシュ分割」P6参照。ただし質量は1としている。
visY[k]=1.414*sqrt(ky);
Ax[k]=(Fx[k]-visX[k]*U[k]);
Ay[k]=(Fy[k]-visY[k]*V[k]);
}//各粒子の加速度が求まった。
if(t==0) //最初のｽﾃｯﾌﾟ時にdtを決定
{
double MaxAccel=0;
for(int k=0;k<newN;k++)
{
double accel2=Ax[k]*Ax[k]+Ay[k]*Ay[k];
if(accel2>MaxAccel) MaxAccel=accel2;
}
MaxAccel=sqrt(MaxAccel);//最大加速度が求まった
dt=sqrt(0.02*le/MaxAccel);
}
for(int k=0;k<newN;k++)//速度と位置の更新
{
int i=beforeN+k;
double u=U[k];
double v=V[k];
U[k]+=dt*Ax[k];
V[k]+=dt*Ay[k];
PART[i].Add(dt*(U[k]+u)*0.5, dt*(V[k]+v)*0.5, 0);
}
//再近接距離がle以下の場合はこれを修正
for(int k=0;k<newN;k++)
{
int i=beforeN+k; //対応する粒子番号
int G_id=index[k]; //格納する格子番号
double mindis=le;
int J=k; //最近接距離の相手粒子
for(int II=G_id-1;II<=G_id+1;II++)
{
for(int JJ=-1*grid_sizeX;JJ<=grid_sizeX;JJ+=grid_sizeX)
{
int M_id=II+JJ;
for(int L=0;L<MESH[M_id].size();L++)
{
int j=MESH[M_id][L];
double x=PART[j].Get_X()-PART[i].Get_X();
double y=PART[j].Get_Y()-PART[i].Get_Y();
double dis=sqrt(x*x+y*y);
if(dis<mindis && i!=j)
{
mindis=dis;
J=j;
}
}
}
}
if(J!=i && J<beforeN)//leより近接している相手が境界粒子なら
{
double L=le-mindis;//開くべき距離
double dX=PART[J].Get_X()-PART[i].Get_X();
double dY=PART[J].Get_Y()-PART[i].Get_Y();
PART[i].Add(-(dX/mindis*L), -(dY/mindis*L), 0);
}
else if(J!=i && J>=beforeN)//leより近接している相手が内部粒子なら
{
double L=0.5*(le-mindis);//開くべき距離
double dX=PART[J].Get_X()-PART[i].Get_X();
double dY=PART[J].Get_Y()-PART[i].Get_Y();
PART[i].Add(-(dX/mindis*L), -(dY/mindis*L), 0);
PART[J].Add(dX/mindis*L, dY/mindis*L, 0);
}
}//////////*/
}/////MD終了
delete [] index;
// delete [] MESH;
}
