void Na2DViewer::paintCrosshair(QPainter& painter)
{
float scale = defaultScale * cameraModel.scale();
QBrush brush1(Qt::black);
QBrush brush2(QColor(255, 255, 180));
QPen pen1(brush1, 2.0/scale);
QPen pen2(brush2, 1.0/scale);
// qDebug() << "paint crosshair";
// Q: Why all this complicated math instead of just [width()/2, height()/2]?
// A: This helps debug/document placement of image focus
qreal w2 = (pixmap.width() - 1.0) / 2.0; // origin at pixel center, not corner
qreal h2 = (pixmap.height() - 1.0) / 2.0; // origin at pixel center, not corner
qreal cx = w2 + flip_X * (cameraModel.focus().x() - w2) + 0.5;
qreal cy = h2 + flip_Y * (cameraModel.focus().y() - h2) + 0.5;
QPointF f(cx, cy);
QPointF dx1(4.0 / scale, 0);
QPointF dy1(0, 4.0 / scale);
QPointF dx2(10.0 / scale, 0); // crosshair size is ten pixels
QPointF dy2(0, 10.0 / scale);
painter.setPen(pen1);
painter.drawLine(f + dx1, f + dx2);
painter.drawLine(f - dx1, f - dx2);
painter.drawLine(f + dy1, f + dy2);
painter.drawLine(f - dy1, f - dy2);
painter.setPen(pen2);
painter.drawLine(f + dx1, f + dx2);
painter.drawLine(f - dx1, f - dx2);
painter.drawLine(f + dy1, f + dy2);
painter.drawLine(f - dy1, f - dy2);
}
pointField quadrilateralDistribution::evaluate()
{
// Read needed material
label N0( readLabel(pointDict_.lookup("N0")) );
label N1( readLabel(pointDict_.lookup("N1")) );
point xs0( pointDict_.lookup("linestart0") );
point xe0( pointDict_.lookup("lineend0") );
point xe1( pointDict_.lookup("lineend1") );
scalar stretch0( pointDict_.lookupOrDefault<scalar>("stretch0", 1.0) );
scalar stretch1( pointDict_.lookupOrDefault<scalar>("stretch1", 1.0) );
// Define the return field
pointField res(N0*N1, xs0);
// Compute the scaling factor
scalar factor0(0.0);
scalar factor1(0.0);
for (int i=1; i < N0; i++)
{
factor0 += Foam::pow( stretch0, static_cast<scalar>(i) );
}
for (int i=1; i < N1; i++)
{
factor1 += Foam::pow( stretch1, static_cast<scalar>(i) );
}
point dx0( (xe0 - xs0)/factor0 );
point dx1( (xe1 - xs0)/factor1 );
// Compute points
for (int j=0; j < N1; j++)
{
if (j != 0)
{
res[j*N0] = res[(j - 1)*N0]
+ Foam::pow( stretch1, static_cast<scalar>(j))*dx1;
}
for (int i=1; i < N0; i++)
{
res[i + j*N0] = res[i - 1 + j*N0]
+ Foam::pow( stretch0, static_cast<scalar>(i) )*dx0;
}
}
return res;
}
bool TextureBoostedSaturatedGradientDataTest(bool create, int width, int height, const Func1 & f)
{
bool result = true;
Data data(f.description);
TEST_LOG_SS(Info, (create ? "Create" : "Verify") << " test " << f.description << " [" << width << ", " << height << "].");
View src(width, height, View::Gray8, NULL, TEST_ALIGN(width));
View dx1(width, height, View::Gray8, NULL, TEST_ALIGN(width));
View dy1(width, height, View::Gray8, NULL, TEST_ALIGN(width));
View dx2(width, height, View::Gray8, NULL, TEST_ALIGN(width));
View dy2(width, height, View::Gray8, NULL, TEST_ALIGN(width));
const int saturation = 16, boost = 4;
if(create)
{
FillRandom(src);
TEST_SAVE(src);
f.Call(src, saturation, boost, dx1, dy1);
TEST_SAVE(dx1);
TEST_SAVE(dy1);
}
else
{
TEST_LOAD(src);
TEST_LOAD(dx1);
TEST_LOAD(dy1);
f.Call(src, saturation, boost, dx2, dy2);
TEST_SAVE(dx2);
TEST_SAVE(dy2);
result = result && Compare(dx1, dx2, 0, true, 32, 0, "dx");
result = result && Compare(dy1, dy2, 0, true, 32, 0, "dy");
}
return result;
}
bool TextureBoostedSaturatedGradientAutoTest(int width, int height, int saturation, int boost, const Func1 & f1, const Func1 & f2)
{
bool result = true;
TEST_LOG_SS(Info, "Test " << f1.description << " & " << f2.description << " [" << width << ", " << height << "] <" << saturation << ", " << boost << ">.");
View src(width, height, View::Gray8, NULL, TEST_ALIGN(width));
FillRandom(src);
View dx1(width, height, View::Gray8, NULL, TEST_ALIGN(width));
View dy1(width, height, View::Gray8, NULL, TEST_ALIGN(width));
View dx2(width, height, View::Gray8, NULL, TEST_ALIGN(width));
View dy2(width, height, View::Gray8, NULL, TEST_ALIGN(width));
TEST_EXECUTE_AT_LEAST_MIN_TIME(f1.Call(src, saturation, boost, dx1, dy1));
TEST_EXECUTE_AT_LEAST_MIN_TIME(f2.Call(src, saturation, boost, dx2, dy2));
result = result && Compare(dx1, dx2, 0, true, 32, 0, "dx");
result = result && Compare(dy1, dy2, 0, true, 32, 0, "dy");
return result;
}
// ** Temporary version
int
ClpPdco::pdco( ClpPdcoBase * stuff, Options &options, Info &info, Outfo &outfo)
{
// D1, D2 are positive-definite diagonal matrices defined from d1, d2.
// In particular, d2 indicates the accuracy required for
// satisfying each row of Ax = b.
//
// D1 and D2 (via d1 and d2) provide primal and dual regularization
// respectively. They ensure that the primal and dual solutions
// (x,r) and (y,z) are unique and bounded.
//
// A scalar d1 is equivalent to d1 = ones(n,1), D1 = diag(d1).
// A scalar d2 is equivalent to d2 = ones(m,1), D2 = diag(d2).
// Typically, d1 = d2 = 1e-4.
// These values perturb phi(x) only slightly (by about 1e-8) and request
// that A*x = b be satisfied quite accurately (to about 1e-8).
// Set d1 = 1e-4, d2 = 1 for least-squares problems with bound constraints.
// The problem is then
//
// minimize phi(x) + 1/2 norm(d1*x)^2 + 1/2 norm(A*x - b)^2
// subject to bl <= x <= bu.
//
// More generally, d1 and d2 may be n and m vectors containing any positive
// values (preferably not too small, and typically no larger than 1).
// Bigger elements of d1 and d2 improve the stability of the solver.
//
// At an optimal solution, if x(j) is on its lower or upper bound,
// the corresponding z(j) is positive or negative respectively.
// If x(j) is between its bounds, z(j) = 0.
// If bl(j) = bu(j), x(j) is fixed at that value and z(j) may have
// either sign.
//
// Also, r and y satisfy r = D2 y, so that Ax + D2^2 y = b.
// Thus if d2(i) = 1e-4, the i-th row of Ax = b will be satisfied to
// approximately 1e-8. This determines how large d2(i) can safely be.
//
//
// EXTERNAL FUNCTIONS:
// options = pdcoSet; provided with pdco.m
// [obj,grad,hess] = pdObj( x ); provided by user
// y = pdMat( name,mode,m,n,x ); provided by user if pdMat
// is a string, not a matrix
//
// INPUT ARGUMENTS:
// pdObj is a string containing the name of a function pdObj.m
// or a function_handle for such a function
// such that [obj,grad,hess] = pdObj(x) defines
// obj = phi(x) : a scalar,
// grad = gradient of phi(x) : an n-vector,
// hess = diag(Hessian of phi): an n-vector.
// Examples:
// If phi(x) is the linear function c"x, pdObj should return
// [obj,grad,hess] = [c"*x, c, zeros(n,1)].
// If phi(x) is the entropy function E(x) = sum x(j) log x(j),
// [obj,grad,hess] = [E(x), log(x)+1, 1./x].
// pdMat may be an ifexplicit m x n matrix A (preferably sparse!),
// or a string containing the name of a function pdMat.m
// or a function_handle for such a function
// such that y = pdMat( name,mode,m,n,x )
// returns y = A*x (mode=1) or y = A"*x (mode=2).
// The input parameter "name" will be the string pdMat.
// b is an m-vector.
// bl is an n-vector of lower bounds. Non-existent bounds
// may be represented by bl(j) = -Inf or bl(j) <= -1e+20.
// bu is an n-vector of upper bounds. Non-existent bounds
// may be represented by bu(j) = Inf or bu(j) >= 1e+20.
// d1, d2 may be positive scalars or positive vectors (see above).
// options is a structure that may be set and altered by pdcoSet
// (type help pdcoSet).
// x0, y0, z0 provide an initial solution.
// xsize, zsize are estimates of the biggest x and z at the solution.
// They are used to scale (x,y,z). Good estimates
// should improve the performance of the barrier method.
//
//
// OUTPUT ARGUMENTS:
// x is the primal solution.
// y is the dual solution associated with Ax + D2 r = b.
// z is the dual solution associated with bl <= x <= bu.
// inform = 0 if a solution is found;
// = 1 if too many iterations were required;
// = 2 if the linesearch failed too often.
// PDitns is the number of Primal-Dual Barrier iterations required.
// CGitns is the number of Conjugate-Gradient iterations required
// if an iterative solver is used (LSQR).
// time is the cpu time used.
//----------------------------------------------------------------------
// PRIVATE FUNCTIONS:
// pdxxxbounds
// pdxxxdistrib
// pdxxxlsqr
// pdxxxlsqrmat
// pdxxxmat
// pdxxxmerit
// pdxxxresid1
// pdxxxresid2
// pdxxxstep
//
// GLOBAL VARIABLES:
// global pdDDD1 pdDDD2 pdDDD3
//
//
// NOTES:
// The matrix A should be reasonably well scaled: norm(A,inf) =~ 1.
// The vector b and objective phi(x) may be of any size, but ensure that
// xsize and zsize are reasonably close to norm(x,inf) and norm(z,inf)
// at the solution.
//
// The files defining pdObj and pdMat
// must not be called Fname.m or Aname.m!!
//
//
// AUTHOR:
// Michael Saunders, Systems Optimization Laboratory (SOL),
// Stanford University, Stanford, California, USA.
// [email protected]
//
// CONTRIBUTORS:
// Byunggyoo Kim, SOL, Stanford University.
// [email protected]
//
// DEVELOPMENT:
// 20 Jun 1997: Original version of pdsco.m derived from pdlp0.m.
// 29 Sep 2002: Original version of pdco.m derived from pdsco.m.
// Introduced D1, D2 in place of gamma*I, delta*I
// and allowed for general bounds bl <= x <= bu.
// 06 Oct 2002: Allowed for fixed variabes: bl(j) = bu(j) for any j.
// 15 Oct 2002: Eliminated some work vectors (since m, n might be LARGE).
// Modularized residuals, linesearch
// 16 Oct 2002: pdxxx..., pdDDD... names rationalized.
// pdAAA eliminated (global copy of A).
// Aname is now used directly as an ifexplicit A or a function.
// NOTE: If Aname is a function, it now has an extra parameter.
// 23 Oct 2002: Fname and Aname can now be function handles.
// 01 Nov 2002: Bug fixed in feval in pdxxxmat.
//-----------------------------------------------------------------------
// global pdDDD1 pdDDD2 pdDDD3
double inf = 1.0e30;
double eps = 1.0e-15;
double atolold = -1.0, r3ratio = -1.0, Pinf, Dinf, Cinf, Cinf0;
printf("\n --------------------------------------------------------");
printf("\n pdco.m Version of 01 Nov 2002");
printf("\n Primal-dual barrier method to minimize a convex function");
printf("\n subject to linear constraints Ax + r = b, bl <= x <= bu");
printf("\n --------------------------------------------------------\n");
int m = numberRows_;
int n = numberColumns_;
bool ifexplicit = true;
CoinDenseVector<double> b(m, rhs_);
CoinDenseVector<double> x(n, x_);
CoinDenseVector<double> y(m, y_);
CoinDenseVector<double> z(n, dj_);
//delete old arrays
delete [] rhs_;
delete [] x_;
delete [] y_;
delete [] dj_;
rhs_ = NULL;
x_ = NULL;
y_ = NULL;
dj_ = NULL;
// Save stuff so available elsewhere
pdcoStuff_ = stuff;
double normb = b.infNorm();
double normx0 = x.infNorm();
double normy0 = y.infNorm();
double normz0 = z.infNorm();
printf("\nmax |b | = %8g max |x0| = %8g", normb , normx0);
printf( " xsize = %8g", xsize_);
printf("\nmax |y0| = %8g max |z0| = %8g", normy0, normz0);
printf( " zsize = %8g", zsize_);
//---------------------------------------------------------------------
// Initialize.
//---------------------------------------------------------------------
//true = 1;
//false = 0;
//zn = zeros(n,1);
//int nb = n + m;
int CGitns = 0;
int inform = 0;
//---------------------------------------------------------------------
// Only allow scalar d1, d2 for now
//---------------------------------------------------------------------
/*
if (d1_->size()==1)
d1_->resize(n, d1_->getElements()[0]); // Allow scalar d1, d2
if (d2_->size()==1)
d2->resize(m, d2->getElements()[0]); // to mean dk * unit vector
*/
assert (stuff->sizeD1() == 1);
double d1 = stuff->getD1();
double d2 = stuff->getD2();
//---------------------------------------------------------------------
// Grab input options.
//---------------------------------------------------------------------
int maxitn = options.MaxIter;
double featol = options.FeaTol;
double opttol = options.OptTol;
double steptol = options.StepTol;
int stepSame = 1; /* options.StepSame; // 1 means stepx == stepz */
double x0min = options.x0min;
double z0min = options.z0min;
double mu0 = options.mu0;
int LSproblem = options.LSproblem; // See below
int LSmethod = options.LSmethod; // 1=Cholesky 2=QR 3=LSQR
int itnlim = options.LSQRMaxIter * CoinMin(m, n);
double atol1 = options.LSQRatol1; // Initial atol
double atol2 = options.LSQRatol2; // Smallest atol,unless atol1 is smaller
double conlim = options.LSQRconlim;
//int wait = options.wait;
// LSproblem:
// 1 = dy 2 = dy shifted, DLS
// 11 = s 12 = s shifted, DLS (dx = Ds)
// 21 = dx
// 31 = 3x3 system, symmetrized by Z^{1/2}
// 32 = 2x2 system, symmetrized by X^{1/2}
//---------------------------------------------------------------------
// Set other parameters.
//---------------------------------------------------------------------
int kminor = 0; // 1 stops after each iteration
double eta = 1e-4; // Linesearch tolerance for "sufficient descent"
double maxf = 10; // Linesearch backtrack limit (function evaluations)
double maxfail = 1; // Linesearch failure limit (consecutive iterations)
double bigcenter = 1e+3; // mu is reduced if center < bigcenter.
// Parameters for LSQR.
double atolmin = eps; // Smallest atol if linesearch back-tracks
double btol = 0; // Should be small (zero is ok)
double show = false; // Controls lsqr iteration log
/*
double gamma = d1->infNorm();
double delta = d2->infNorm();
*/
double gamma = d1;
double delta = d2;
printf("\n\nx0min = %8g featol = %8.1e", x0min, featol);
printf( " d1max = %8.1e", gamma);
printf( "\nz0min = %8g opttol = %8.1e", z0min, opttol);
printf( " d2max = %8.1e", delta);
printf( "\nmu0 = %8.1e steptol = %8g", mu0 , steptol);
printf( " bigcenter= %8g" , bigcenter);
printf("\n\nLSQR:");
printf("\natol1 = %8.1e atol2 = %8.1e", atol1 , atol2 );
printf( " btol = %8.1e", btol );
printf("\nconlim = %8.1e itnlim = %8d" , conlim, itnlim);
printf( " show = %8g" , show );
// LSmethod = 3; ////// Hardwire LSQR
// LSproblem = 1; ////// and LS problem defining "dy".
/*
if wait
printf("\n\nReview parameters... then type "return"\n")
keyboard
end
*/
if (eta < 0)
printf("\n\nLinesearch disabled by eta < 0");
//---------------------------------------------------------------------
// All parameters have now been set.
//---------------------------------------------------------------------
double time = CoinCpuTime();
//bool useChol = (LSmethod == 1);
//bool useQR = (LSmethod == 2);
bool direct = (LSmethod <= 2 && ifexplicit);
char solver[7];
strncpy(solver, " LSQR", 7);
//---------------------------------------------------------------------
// Categorize bounds and allow for fixed variables by modifying b.
//---------------------------------------------------------------------
int nlow, nupp, nfix;
int *bptrs[3] = {0};
getBoundTypes(&nlow, &nupp, &nfix, bptrs );
int *low = bptrs[0];
int *upp = bptrs[1];
int *fix = bptrs[2];
int nU = n;
if (nupp == 0) nU = 1; //Make dummy vectors if no Upper bounds
//---------------------------------------------------------------------
// Get pointers to local copy of model bounds
//---------------------------------------------------------------------
CoinDenseVector<double> bl(n, columnLower_);
double *bl_elts = bl.getElements();
CoinDenseVector<double> bu(nU, columnUpper_); // this is dummy if no UB
double *bu_elts = bu.getElements();
CoinDenseVector<double> r1(m, 0.0);
double *r1_elts = r1.getElements();
CoinDenseVector<double> x1(n, 0.0);
double *x1_elts = x1.getElements();
if (nfix > 0) {
for (int k = 0; k < nfix; k++)
x1_elts[fix[k]] = bl[fix[k]];
matVecMult(1, r1, x1);
b = b - r1;
// At some stage, might want to look at normfix = norm(r1,inf);
}
//---------------------------------------------------------------------
// Scale the input data.
// The scaled variables are
// xbar = x/beta,
// ybar = y/zeta,
// zbar = z/zeta.
// Define
// theta = beta*zeta;
// The scaled function is
// phibar = ( 1 /theta) fbar(beta*xbar),
// gradient = (beta /theta) grad,
// Hessian = (beta2/theta) hess.
//---------------------------------------------------------------------
double beta = xsize_;
if (beta == 0) beta = 1; // beta scales b, x.
double zeta = zsize_;
if (zeta == 0) zeta = 1; // zeta scales y, z.
double theta = beta * zeta; // theta scales obj.
// (theta could be anything, but theta = beta*zeta makes
// scaled grad = grad/zeta = 1 approximately if zeta is chosen right.)
for (int k = 0; k < nlow; k++)
bl_elts[low[k]] = bl_elts[low[k]] / beta;
for (int k = 0; k < nupp; k++)
bu_elts[upp[k]] = bu_elts[upp[k]] / beta;
d1 = d1 * ( beta / sqrt(theta) );
d2 = d2 * ( sqrt(theta) / beta );
double beta2 = beta * beta;
b.scale( (1.0 / beta) );
y.scale( (1.0 / zeta) );
x.scale( (1.0 / beta) );
z.scale( (1.0 / zeta) );
//---------------------------------------------------------------------
// Initialize vectors that are not fully used if bounds are missing.
//---------------------------------------------------------------------
CoinDenseVector<double> rL(n, 0.0);
CoinDenseVector<double> cL(n, 0.0);
CoinDenseVector<double> z1(n, 0.0);
CoinDenseVector<double> dx1(n, 0.0);
CoinDenseVector<double> dz1(n, 0.0);
CoinDenseVector<double> r2(n, 0.0);
// Assign upper bd regions (dummy if no UBs)
CoinDenseVector<double> rU(nU, 0.0);
CoinDenseVector<double> cU(nU, 0.0);
CoinDenseVector<double> x2(nU, 0.0);
CoinDenseVector<double> z2(nU, 0.0);
CoinDenseVector<double> dx2(nU, 0.0);
CoinDenseVector<double> dz2(nU, 0.0);
//---------------------------------------------------------------------
// Initialize x, y, z, objective, etc.
//---------------------------------------------------------------------
CoinDenseVector<double> dx(n, 0.0);
CoinDenseVector<double> dy(m, 0.0);
CoinDenseVector<double> Pr(m);
CoinDenseVector<double> D(n);
double *D_elts = D.getElements();
CoinDenseVector<double> w(n);
double *w_elts = w.getElements();
CoinDenseVector<double> rhs(m + n);
//---------------------------------------------------------------------
// Pull out the element array pointers for efficiency
//---------------------------------------------------------------------
double *x_elts = x.getElements();
double *x2_elts = x2.getElements();
double *z_elts = z.getElements();
double *z1_elts = z1.getElements();
double *z2_elts = z2.getElements();
for (int k = 0; k < nlow; k++) {
x_elts[low[k]] = CoinMax( x_elts[low[k]], bl[low[k]]);
x1_elts[low[k]] = CoinMax( x_elts[low[k]] - bl[low[k]], x0min );
z1_elts[low[k]] = CoinMax( z_elts[low[k]], z0min );
}
for (int k = 0; k < nupp; k++) {
x_elts[upp[k]] = CoinMin( x_elts[upp[k]], bu[upp[k]]);
x2_elts[upp[k]] = CoinMax(bu[upp[k]] - x_elts[upp[k]], x0min );
z2_elts[upp[k]] = CoinMax(-z_elts[upp[k]], z0min );
}
//////////////////// Assume hessian is diagonal. //////////////////////
// [obj,grad,hess] = feval( Fname, (x*beta) );
x.scale(beta);
double obj = getObj(x);
CoinDenseVector<double> grad(n);
getGrad(x, grad);
CoinDenseVector<double> H(n);
getHessian(x , H);
x.scale((1.0 / beta));
//double * g_elts = grad.getElements();
double * H_elts = H.getElements();
obj /= theta; // Scaled obj.
grad = grad * (beta / theta) + (d1 * d1) * x; // grad includes x regularization.
H = H * (beta2 / theta) + (d1 * d1) ; // H includes x regularization.
/*---------------------------------------------------------------------
// Compute primal and dual residuals:
// r1 = b - Aprod(x) - d2*d2*y;
// r2 = grad - Atprod(y) + z2 - z1;
// rL = bl - x + x1;
// rU = x + x2 - bu; */
//---------------------------------------------------------------------
// [r1,r2,rL,rU,Pinf,Dinf] = ...
// pdxxxresid1( Aname,fix,low,upp, ...
// b,bl,bu,d1,d2,grad,rL,rU,x,x1,x2,y,z1,z2 );
pdxxxresid1( this, nlow, nupp, nfix, low, upp, fix,
b, bl_elts, bu_elts, d1, d2, grad, rL, rU, x, x1, x2, y, z1, z2,
r1, r2, &Pinf, &Dinf);
//---------------------------------------------------------------------
// Initialize mu and complementarity residuals:
// cL = mu*e - X1*z1.
// cU = mu*e - X2*z2.
//
// 25 Jan 2001: Now that b and obj are scaled (and hence x,y,z),
// we should be able to use mufirst = mu0 (absolute value).
// 0.1 worked poorly on StarTest1 with x0min = z0min = 0.1.
// 29 Jan 2001: We might as well use mu0 = x0min * z0min;
// so that most variables are centered after a warm start.
// 29 Sep 2002: Use mufirst = mu0*(x0min * z0min),
// regarding mu0 as a scaling of the initial center.
//---------------------------------------------------------------------
// double mufirst = mu0*(x0min * z0min);
double mufirst = mu0; // revert to absolute value
double mulast = 0.1 * opttol;
mulast = CoinMin( mulast, mufirst );
double mu = mufirst;
double center, fmerit;
pdxxxresid2( mu, nlow, nupp, low, upp, cL, cU, x1, x2,
z1, z2, ¢er, &Cinf, &Cinf0 );
fmerit = pdxxxmerit(nlow, nupp, low, upp, r1, r2, rL, rU, cL, cU );
// Initialize other things.
bool precon = true;
double PDitns = 0;
//bool converged = false;
double atol = atol1;
atol2 = CoinMax( atol2, atolmin );
atolmin = atol2;
// pdDDD2 = d2; // Global vector for diagonal matrix D2
// Iteration log.
int nf = 0;
int itncg = 0;
int nfail = 0;
printf("\n\nItn mu stepx stepz Pinf Dinf");
printf(" Cinf Objective nf center");
if (direct) {
printf("\n");
} else {
printf(" atol solver Inexact\n");
}
double regx = (d1 * x).twoNorm();
double regy = (d2 * y).twoNorm();
// regterm = twoNorm(d1.*x)^2 + norm(d2.*y)^2;
double regterm = regx * regx + regy * regy;
double objreg = obj + 0.5 * regterm;
double objtrue = objreg * theta;
printf("\n%3g ", PDitns );
printf("%6.1f%6.1f" , log10(Pinf ), log10(Dinf));
printf("%6.1f%15.7e", log10(Cinf0), objtrue );
printf(" %8.1f\n" , center );
/*
if kminor
printf("\n\nStart of first minor itn...\n");
keyboard
end
*/
//---------------------------------------------------------------------
// Main loop.
//---------------------------------------------------------------------
// Lsqr
ClpLsqr thisLsqr(this);
// while (converged) {
while(PDitns < maxitn) {
PDitns = PDitns + 1;
// 31 Jan 2001: Set atol according to progress, a la Inexact Newton.
// 07 Feb 2001: 0.1 not small enough for Satellite problem. Try 0.01.
// 25 Apr 2001: 0.01 seems wasteful for Star problem.
// Now that starting conditions are better, go back to 0.1.
double r3norm = CoinMax(Pinf, CoinMax(Dinf, Cinf));
atol = CoinMin(atol, r3norm * 0.1);
atol = CoinMax(atol, atolmin );
info.r3norm = r3norm;
//-------------------------------------------------------------------
// Define a damped Newton iteration for solving f = 0,
// keeping x1, x2, z1, z2 > 0. We eliminate dx1, dx2, dz1, dz2
// to obtain the system
//
// [-H2 A" ] [ dx ] = [ w ], H2 = H + D1^2 + X1inv Z1 + X2inv Z2,
// [ A D2^2] [ dy ] = [ r1] w = r2 - X1inv(cL + Z1 rL)
// + X2inv(cU + Z2 rU),
//
// which is equivalent to the least-squares problem
//
// min || [ D A"]dy - [ D w ] ||, D = H2^{-1/2}. (*)
// || [ D2 ] [D2inv r1] ||
//-------------------------------------------------------------------
for (int k = 0; k < nlow; k++)
H_elts[low[k]] = H_elts[low[k]] + z1[low[k]] / x1[low[k]];
for (int k = 0; k < nupp; k++)
H[upp[k]] = H[upp[k]] + z2[upp[k]] / x2[upp[k]];
w = r2;
for (int k = 0; k < nlow; k++)
w[low[k]] = w[low[k]] - (cL[low[k]] + z1[low[k]] * rL[low[k]]) / x1[low[k]];
for (int k = 0; k < nupp; k++)
w[upp[k]] = w[upp[k]] + (cU[upp[k]] + z2[upp[k]] * rU[upp[k]]) / x2[upp[k]];
if (LSproblem == 1) {
//-----------------------------------------------------------------
// Solve (*) for dy.
//-----------------------------------------------------------------
H = 1.0 / H; // H is now Hinv (NOTE!)
for (int k = 0; k < nfix; k++)
H[fix[k]] = 0;
for (int k = 0; k < n; k++)
D_elts[k] = sqrt(H_elts[k]);
thisLsqr.borrowDiag1(D_elts);
thisLsqr.diag2_ = d2;
if (direct) {
// Omit direct option for now
} else {// Iterative solve using LSQR.
//rhs = [ D.*w; r1./d2 ];
for (int k = 0; k < n; k++)
rhs[k] = D_elts[k] * w_elts[k];
for (int k = 0; k < m; k++)
rhs[n+k] = r1_elts[k] * (1.0 / d2);
double damp = 0;
if (precon) { // Construct diagonal preconditioner for LSQR
matPrecon(d2, Pr, D);
}
/*
rw(7) = precon;
info.atolmin = atolmin;
info.r3norm = fmerit; // Must be the 2-norm here.
[ dy, istop, itncg, outfo ] = ...
pdxxxlsqr( nb,m,"pdxxxlsqrmat",Aname,rw,rhs,damp, ...
atol,btol,conlim,itnlim,show,info );
thisLsqr.input->rhs_vec = &rhs;
thisLsqr.input->sol_vec = &dy;
thisLsqr.input->rel_mat_err = atol;
thisLsqr.do_lsqr(this);
*/
// New version of lsqr
int istop;
dy.clear();
show = false;
info.atolmin = atolmin;
info.r3norm = fmerit; // Must be the 2-norm here.
thisLsqr.do_lsqr( rhs, damp, atol, btol, conlim, itnlim,
show, info, dy , &istop, &itncg, &outfo, precon, Pr);
if (precon)
dy = dy * Pr;
if (!precon && itncg > 999999)
precon = true;
if (istop == 3 || istop == 7 ) // conlim or itnlim
printf("\n LSQR stopped early: istop = //%d", istop);
atolold = outfo.atolold;
atol = outfo.atolnew;
r3ratio = outfo.r3ratio;
}// LSproblem 1
// grad = pdxxxmat( Aname,2,m,n,dy ); // grad = A"dy
grad.clear();
matVecMult(2, grad, dy);
for (int k = 0; k < nfix; k++)
grad[fix[k]] = 0; // grad is a work vector
dx = H * (grad - w);
} else {
perror( "This LSproblem not yet implemented\n" );
}
//-------------------------------------------------------------------
CGitns += itncg;
//-------------------------------------------------------------------
// dx and dy are now known. Get dx1, dx2, dz1, dz2.
//-------------------------------------------------------------------
for (int k = 0; k < nlow; k++) {
dx1[low[k]] = - rL[low[k]] + dx[low[k]];
dz1[low[k]] = (cL[low[k]] - z1[low[k]] * dx1[low[k]]) / x1[low[k]];
}
for (int k = 0; k < nupp; k++) {
dx2[upp[k]] = - rU[upp[k]] - dx[upp[k]];
dz2[upp[k]] = (cU[upp[k]] - z2[upp[k]] * dx2[upp[k]]) / x2[upp[k]];
}
//-------------------------------------------------------------------
// Find the maximum step.
//--------------------------------------------------------------------
double stepx1 = pdxxxstep(nlow, low, x1, dx1 );
double stepx2 = inf;
if (nupp > 0)
stepx2 = pdxxxstep(nupp, upp, x2, dx2 );
double stepz1 = pdxxxstep( z1 , dz1 );
double stepz2 = inf;
if (nupp > 0)
stepz2 = pdxxxstep( z2 , dz2 );
double stepx = CoinMin( stepx1, stepx2 );
double stepz = CoinMin( stepz1, stepz2 );
stepx = CoinMin( steptol * stepx, 1.0 );
stepz = CoinMin( steptol * stepz, 1.0 );
if (stepSame) { // For NLPs, force same step
stepx = CoinMin( stepx, stepz ); // (true Newton method)
stepz = stepx;
}
//-------------------------------------------------------------------
// Backtracking linesearch.
//-------------------------------------------------------------------
bool fail = true;
nf = 0;
while (nf < maxf) {
nf = nf + 1;
x = x + stepx * dx;
y = y + stepz * dy;
for (int k = 0; k < nlow; k++) {
x1[low[k]] = x1[low[k]] + stepx * dx1[low[k]];
z1[low[k]] = z1[low[k]] + stepz * dz1[low[k]];
}
for (int k = 0; k < nupp; k++) {
x2[upp[k]] = x2[upp[k]] + stepx * dx2[upp[k]];
z2[upp[k]] = z2[upp[k]] + stepz * dz2[upp[k]];
}
// [obj,grad,hess] = feval( Fname, (x*beta) );
x.scale(beta);
obj = getObj(x);
getGrad(x, grad);
getHessian(x, H);
x.scale((1.0 / beta));
obj /= theta;
grad = grad * (beta / theta) + d1 * d1 * x;
H = H * (beta2 / theta) + d1 * d1;
// [r1,r2,rL,rU,Pinf,Dinf] = ...
pdxxxresid1( this, nlow, nupp, nfix, low, upp, fix,
b, bl_elts, bu_elts, d1, d2, grad, rL, rU, x, x1, x2,
y, z1, z2, r1, r2, &Pinf, &Dinf );
//double center, Cinf, Cinf0;
// [cL,cU,center,Cinf,Cinf0] = ...
pdxxxresid2( mu, nlow, nupp, low, upp, cL, cU, x1, x2, z1, z2,
¢er, &Cinf, &Cinf0);
double fmeritnew = pdxxxmerit(nlow, nupp, low, upp, r1, r2, rL, rU, cL, cU );
double step = CoinMin( stepx, stepz );
if (fmeritnew <= (1 - eta * step)*fmerit) {
fail = false;
break;
}
// Merit function didn"t decrease.
// Restore variables to previous values.
// (This introduces a little error, but save lots of space.)
x = x - stepx * dx;
y = y - stepz * dy;
for (int k = 0; k < nlow; k++) {
x1[low[k]] = x1[low[k]] - stepx * dx1[low[k]];
z1[low[k]] = z1[low[k]] - stepz * dz1[low[k]];
}
for (int k = 0; k < nupp; k++) {
x2[upp[k]] = x2[upp[k]] - stepx * dx2[upp[k]];
z2[upp[k]] = z2[upp[k]] - stepz * dz2[upp[k]];
}
// Back-track.
// If it"s the first time,
// make stepx and stepz the same.
if (nf == 1 && stepx != stepz) {
stepx = step;
} else if (nf < maxf) {
stepx = stepx / 2;
}
stepz = stepx;
}
if (fail) {
printf("\n Linesearch failed (nf too big)");
nfail += 1;
} else {
nfail = 0;
}
//-------------------------------------------------------------------
// Set convergence measures.
//--------------------------------------------------------------------
regx = (d1 * x).twoNorm();
regy = (d2 * y).twoNorm();
regterm = regx * regx + regy * regy;
objreg = obj + 0.5 * regterm;
objtrue = objreg * theta;
bool primalfeas = Pinf <= featol;
bool dualfeas = Dinf <= featol;
bool complementary = Cinf0 <= opttol;
bool enough = PDitns >= 4; // Prevent premature termination.
bool converged = primalfeas & dualfeas & complementary & enough;
//-------------------------------------------------------------------
// Iteration log.
//-------------------------------------------------------------------
char str1[100], str2[100], str3[100], str4[100], str5[100];
sprintf(str1, "\n%3g%5.1f" , PDitns , log10(mu) );
sprintf(str2, "%8.5f%8.5f" , stepx , stepz );
if (stepx < 0.0001 || stepz < 0.0001) {
sprintf(str2, " %6.1e %6.1e" , stepx , stepz );
}
sprintf(str3, "%6.1f%6.1f" , log10(Pinf) , log10(Dinf));
sprintf(str4, "%6.1f%15.7e", log10(Cinf0), objtrue );
sprintf(str5, "%3d%8.1f" , nf , center );
if (center > 99999) {
sprintf(str5, "%3d%8.1e" , nf , center );
}
printf("%s%s%s%s%s", str1, str2, str3, str4, str5);
if (direct) {
// relax
} else {
printf(" %5.1f%7d%7.3f", log10(atolold), itncg, r3ratio);
}
//-------------------------------------------------------------------
// Test for termination.
//-------------------------------------------------------------------
if (kminor) {
printf( "\nStart of next minor itn...\n");
// keyboard;
}
if (converged) {
printf("\n Converged");
break;
} else if (PDitns >= maxitn) {
printf("\n Too many iterations");
inform = 1;
break;
} else if (nfail >= maxfail) {
printf("\n Too many linesearch failures");
inform = 2;
break;
} else {
// Reduce mu, and reset certain residuals.
double stepmu = CoinMin( stepx , stepz );
stepmu = CoinMin( stepmu, steptol );
double muold = mu;
mu = mu - stepmu * mu;
if (center >= bigcenter)
mu = muold;
// mutrad = mu0*(sum(Xz)/n); // 24 May 1998: Traditional value, but
// mu = CoinMin(mu,mutrad ); // it seemed to decrease mu too much.
mu = CoinMax(mu, mulast); // 13 Jun 1998: No need for smaller mu.
// [cL,cU,center,Cinf,Cinf0] = ...
pdxxxresid2( mu, nlow, nupp, low, upp, cL, cU, x1, x2, z1, z2,
¢er, &Cinf, &Cinf0 );
fmerit = pdxxxmerit( nlow, nupp, low, upp, r1, r2, rL, rU, cL, cU );
// Reduce atol for LSQR (and SYMMLQ).
// NOW DONE AT TOP OF LOOP.
atolold = atol;
// if atol > atol2
// atolfac = (mu/mufirst)^0.25;
// atol = CoinMax( atol*atolfac, atol2 );
// end
// atol = CoinMin( atol, mu ); // 22 Jan 2001: a la Inexact Newton.
// atol = CoinMin( atol, 0.5*mu ); // 30 Jan 2001: A bit tighter
// If the linesearch took more than one function (nf > 1),
// we assume the search direction needed more accuracy
// (though this may be true only for LPs).
// 12 Jun 1998: Ask for more accuracy if nf > 2.
// 24 Nov 2000: Also if the steps are small.
// 30 Jan 2001: Small steps might be ok with warm start.
// 06 Feb 2001: Not necessarily. Reinstated tests in next line.
if (nf > 2 || CoinMin( stepx, stepz ) <= 0.01)
atol = atolold * 0.1;
}
//---------------------------------------------------------------------
// End of main loop.
//---------------------------------------------------------------------
}
for (int k = 0; k < nfix; k++)
x[fix[k]] = bl[fix[k]];
z = z1;
if (nupp > 0)
z = z - z2;
printf("\n\nmax |x| =%10.3f", x.infNorm() );
printf(" max |y| =%10.3f", y.infNorm() );
printf(" max |z| =%10.3f", z.infNorm() );
printf(" scaled");
x.scale(beta);
y.scale(zeta);
z.scale(zeta); // Unscale x, y, z.
printf( "\nmax |x| =%10.3f", x.infNorm() );
printf(" max |y| =%10.3f", y.infNorm() );
printf(" max |z| =%10.3f", z.infNorm() );
printf(" unscaled\n");
time = CoinCpuTime() - time;
char str1[100], str2[100];
sprintf(str1, "\nPDitns =%10g", PDitns );
sprintf(str2, "itns =%10d", CGitns );
// printf( [str1 " " solver str2] );
printf(" time =%10.1f\n", time);
/*
pdxxxdistrib( abs(x),abs(z) ); // Private function
if (wait)
keyboard;
*/
//-----------------------------------------------------------------------
// End function pdco.m
//-----------------------------------------------------------------------
/* printf("Solution x values:\n\n");
for (int k=0; k<n; k++)
printf(" %d %e\n", k, x[k]);
*/
// Print distribution
double thresh[9] = { 0.00000001, 0.0000001, 0.000001, 0.00001, 0.0001, 0.001, 0.01, 0.1, 1.00001};
int counts[9] = {0};
for (int ij = 0; ij < n; ij++) {
for (int j = 0; j < 9; j++) {
if(x[ij] < thresh[j]) {
counts[j] += 1;
break;
}
}
}
printf ("Distribution of Solution Values\n");
for (int j = 8; j > 1; j--)
printf(" %g to %g %d\n", thresh[j-1], thresh[j], counts[j]);
printf(" Less than %g %d\n", thresh[2], counts[0]);
return inform;
}
bool HomotopyConcrete< RT, FixedPrecisionHomotopyAlgorithm >::track(const MutableMatrix* inputs, MutableMatrix* outputs,
MutableMatrix* output_extras,
gmp_RR init_dt, gmp_RR min_dt,
gmp_RR epsilon, // o.CorrectorTolerance,
int max_corr_steps,
gmp_RR infinity_threshold
)
{
std::chrono::steady_clock::time_point start = std::chrono::steady_clock::now();
size_t solveLinearTime = 0, solveLinearCount = 0, evaluateTime = 0;
// std::cout << "inside HomotopyConcrete<RT,FixedPrecisionHomotopyAlgorithm>::track" << std::endl;
// double the_smallest_number = 1e-13;
const Ring* matRing = inputs->get_ring();
if (outputs->get_ring()!= matRing) {
ERROR("outputs and inputs are in different rings");
return false;
}
auto inp = dynamic_cast<const MutableMat< DMat<RT> >*>(inputs);
auto out = dynamic_cast<MutableMat< DMat<RT> >*>(outputs);
auto out_extras = dynamic_cast<MutableMat< DMat<M2::ARingZZGMP> >*>(output_extras);
if (inp == nullptr) {
ERROR("inputs: expected a dense mutable matrix");
return false;
}
if (out == nullptr) {
ERROR("outputs: expected a dense mutable matrix");
return false;
}
if (out_extras == nullptr) {
ERROR("output_extras: expected a dense mutable matrix");
return false;
}
auto& in = inp->getMat();
auto& ou = out->getMat();
auto& oe = out_extras->getMat();
size_t n_sols = in.numColumns();
size_t n = in.numRows()-1; // number of x vars
if (ou.numColumns() != n_sols or ou.numRows() != n+2) {
ERROR("output: wrong shape");
return false;
}
if (oe.numColumns() != n_sols or oe.numRows() != 2) {
ERROR("output_extras: wrong shape");
return false;
}
const RT& C = in.ring();
typename RT::RealRingType R = C.real_ring();
typedef typename RT::ElementType ElementType;
typedef typename RT::RealRingType::ElementType RealElementType;
typedef MatElementaryOps< DMat< RT > > MatOps;
RealElementType t_step;
RealElementType min_step2;
RealElementType epsilon2;
RealElementType infinity_threshold2;
R.init(t_step);
R.init(min_step2);
R.init(epsilon2);
R.init(infinity_threshold2);
R.set_from_BigReal(t_step,init_dt); // initial step
R.set_from_BigReal(min_step2,min_dt);
R.mult(min_step2, min_step2, min_step2); //min_step^2
R.set_from_BigReal(epsilon2,epsilon);
int tolerance_bits = -R.log2abs(epsilon2);
R.mult(epsilon2, epsilon2, epsilon2); //epsilon^2
R.set_from_BigReal(infinity_threshold2,infinity_threshold);
R.mult(infinity_threshold2, infinity_threshold2, infinity_threshold2);
int num_successes_before_increase = 3;
RealElementType t0,dt,one_minus_t0,dx_norm2,x_norm2,abs2dc;
R.init(t0);
R.init(dt);
R.init(one_minus_t0);
R.init(dx_norm2);
R.init(x_norm2);
R.init(abs2dc);
// constants
RealElementType one,two,four,six,one_half,one_sixth;
RealElementType& dt_factor = one_half;
R.init(one);
R.set_from_long(one,1);
R.init(two);
R.set_from_long(two,2);
R.init(four);
R.set_from_long(four,4);
R.init(six);
R.set_from_long(six,6);
R.init(one_half);
R.divide(one_half,one,two);
R.init(one_sixth);
R.divide(one_sixth,one,six);
ElementType c_init,c_end,dc,one_half_dc;
C.init(c_init);
C.init(c_end);
C.init(dc);
C.init(one_half_dc);
// think: x_0..x_(n-1), c
// c = the homotopy continuation parameter "t" upstair, varies on a (staight line) segment of complex plane (from c_init to c_end)
// t = a real running in the interval [0,1]
DMat<RT> x0c0(C,n+1,1);
DMat<RT> x1c1(C,n+1,1);
DMat<RT> xc(C,n+1,1);
DMat<RT> HxH(C,n,n+1);
DMat<RT>& Hxt = HxH; // the matrix has the same shape: reuse memory
DMat<RT> LHSmat(C,n,n);
auto LHS = submatrix(LHSmat);
DMat<RT> RHSmat(C,n,1);
auto RHS = submatrix(RHSmat);
DMat<RT> dx(C,n,1);
DMat<RT> dx1(C,n,1);
DMat<RT> dx2(C,n,1);
DMat<RT> dx3(C,n,1);
DMat<RT> dx4(C,n,1);
DMat<RT> Jinv_times_random(C,n,1);
ElementType& c0 = x0c0.entry(n,0);
ElementType& c1 = x1c1.entry(n,0);
ElementType& c = xc.entry(n,0);
RealElementType& tol2 = epsilon2; // current tolerance squared
bool linearSolve_success;
for(size_t s=0; s<n_sols; s++) {
SolutionStatus status = PROCESSING;
// set initial solution and initial value of the continuation parameter
//for(size_t i=0; i<=n; i++)
// C.set(x0c0.entry(i,0), in.entry(i,s));
submatrix(x0c0) = submatrix(const_cast<DMat<RT>&>(in), 0,s, n+1,1);
C.set(c_init,c0);
C.set(c_end,ou.entry(n,s));
R.set_zero(t0);
bool t0equals1 = false;
// t_step is actually the initial (absolute) length of step on the interval [c_init,c_end]
// dt is an increment for t on the interval [0,1]
R.set(dt,t_step);
C.subtract(dc,c_end,c_init);
C.abs(abs2dc,dc); // don't wnat to create new temporary elts: reusing dc and abs2dc
R.divide(dt,dt,abs2dc);
int predictor_successes = 0;
int count = 0; // number of steps
// track the real segment (1-t)*c0 + t*c1, a\in [0,1]
while (status == PROCESSING and not t0equals1) {
if (M2_numericalAlgebraicGeometryTrace>3) {
buffer o;
R.elem_text_out(o,t0,true,false,false);
std::cout << "t0 = " << o.str();
o.reset();
C.elem_text_out(o,c0,true,false,false);
std::cout << ", c0 = " << o.str() << std::endl;
}
R.subtract(one_minus_t0,one,t0);
if (R.compare_elems(dt,one_minus_t0)>0) {
R.set(dt,one_minus_t0);
t0equals1 = true;
C.subtract(dc,c_end,c0);
C.set(c1,c_end);
} else {
C.subtract(dc,c_end,c0);
C.mult(dc,dc,dt);
C.divide(dc,dc,one_minus_t0);
C.add(c1,c0,dc);
}
// PREDICTOR in: x0c0,dt
// out: dx
/* top-level code for Runge-Kutta-4
dx1 := solveHxTimesDXequalsMinusHt(x0,t0);
dx2 := solveHxTimesDXequalsMinusHt(x0+(1/2)*dx1*dt,t0+(1/2)*dt);
dx3 := solveHxTimesDXequalsMinusHt(x0+(1/2)*dx2*dt,t0+(1/2)*dt);
dx4 := solveHxTimesDXequalsMinusHt(x0+dx3*dt,t0+dt);
(1/6)*dt*(dx1+2*dx2+2*dx3+dx4)
*/
C.mult(one_half_dc, dc, one_half);
// dx1
submatrix(xc) = submatrix(x0c0);
TIME(evaluateTime,
mHxt.evaluate(xc,Hxt)
)
LHS = submatrix(Hxt, 0,0, n,n);
RHS = submatrix(Hxt, 0,n, n,1);
MatrixOps::negateInPlace(RHSmat);
TIME(solveLinearTime,
linearSolve_success = MatrixOps::solveLinear(LHSmat,RHSmat,dx1)
);
solveLinearCount++;
// dx2
if (linearSolve_success) {
submatrix(dx1) *= one_half_dc; // "dx1" := (1/2)*dx1*dt
submatrix(xc, 0,0, n,1) += submatrix(dx1);
C.add(c,c,one_half_dc);
TIME(evaluateTime,
mHxt.evaluate(xc,Hxt)
)
LHS = submatrix(Hxt, 0,0, n,n);
RHS = submatrix(Hxt, 0,n, n,1);
MatrixOps::negateInPlace(RHSmat);
TIME(solveLinearTime,
linearSolve_success = MatrixOps::solveLinear(LHSmat,RHSmat,dx2);
)
solveLinearCount++;
}
// dx3
if (linearSolve_success) {
submatrix(dx2) *= one_half_dc; // "dx2" := (1/2)*dx2*dt
submatrix(xc, 0,0, n,1) = submatrix(x0c0, 0,0, n,1);
submatrix(xc, 0,0, n,1) += submatrix(dx2);
// C.add(c,c,one_half_dc); // c should not change here??? or copy c two lines above???
TIME(evaluateTime,
mHxt.evaluate(xc,Hxt)
);
LHS = submatrix(Hxt, 0,0, n,n);
RHS = submatrix(Hxt, 0,n, n,1);
MatrixOps::negateInPlace(RHSmat);
TIME(solveLinearTime,
linearSolve_success = MatrixOps::solveLinear(LHSmat,RHSmat,dx3);
);
solveLinearCount++;
}
// dx4
if (linearSolve_success) {
submatrix(dx3) *= dc; // "dx3" := dx3*dt
submatrix(xc) = submatrix(x0c0); // sets c=c0 as well (not needed for dx2???,dx3)
submatrix(xc, 0,0, n,1) += submatrix(dx3);
C.add(c,c,dc);
TIME(evaluateTime,
mHxt.evaluate(xc,Hxt)
);
LHS = submatrix(Hxt, 0,0, n,n);
RHS = submatrix(Hxt, 0,n, n,1);
MatrixOps::negateInPlace(RHSmat);
TIME(solveLinearTime,
linearSolve_success = MatrixOps::solveLinear(LHSmat,RHSmat,dx4);
);
solveLinearCount++;
}
void tri_hp_ps::length() {
int i,j,k,v0,v1,v2,indx,sind,tind,count;
TinyVector<FLT,2> dx0,dx1,dx2,ep,dedpsi;
FLT q,p,duv,um,vm,u,v;
FLT sum,ruv,ratio;
FLT length0,length1,length2,lengthept;
FLT ang1,curved1,ang2,curved2;
FLT norm;
gbl->eanda = 0.0;
for(tind=0;tind<ntri;++tind) {
q = 0.0;
p = 0.0;
duv = 0.0;
um = ug.v(tri(tind).pnt(2),0);
vm = ug.v(tri(tind).pnt(2),1);
for(j=0;j<3;++j) {
v0 = tri(tind).pnt(j);
p += fabs(ug.v(v0,2));
duv += fabs(u-um)+fabs(v-vm);
}
gbl->eanda(0) += 1./3.*(p*area(tind) +duv*gbl->mu*sqrt(area(tind)) );
gbl->eanda(1) += area(tind);
}
sim::blks.allreduce(gbl->eanda.data(),gbl->eanda_recv.data(),2,blocks::flt_msg,blocks::sum);
norm = gbl->eanda_recv(0)/gbl->eanda_recv(1);
gbl->fltwk(Range(0,npnt-1)) = 0.0;
switch(basis::tri(log2p)->p()) {
case(1): {
for(i=0;i<nseg;++i) {
v0 = seg(i).pnt(0);
v1 = seg(i).pnt(1);
ruv = gbl->mu/distance(v0,v1);
sum = distance2(v0,v1)*(ruv*(fabs(ug.v(v0,0) -ug.v(v1,0)) +fabs(ug.v(v0,1) -ug.v(v1,1))) +fabs(ug.v(v0,2) -ug.v(v1,2)));
gbl->fltwk(v0) += sum;
gbl->fltwk(v1) += sum;
}
break;
}
default: {
indx = basis::tri(log2p)->sm()-1;
for(i=0;i<nseg;++i) {
v0 = seg(i).pnt(0);
v1 = seg(i).pnt(1);
ruv = +gbl->mu/distance(v0,v1);
sum = distance2(v0,v1)*(ruv*(fabs(ug.s(i,indx,0)) +fabs(ug.s(i,indx,1))) +fabs(ug.s(i,indx,2)));
gbl->fltwk(v0) += sum;
gbl->fltwk(v1) += sum;
}
/* BOUNDARY CURVATURE */
for(i=0;i<nebd;++i) {
if (!(hp_ebdry(i)->is_curved())) continue;
for(j=0;j<ebdry(i)->nseg;++j) {
sind = ebdry(i)->seg(j);
v1 = seg(sind).pnt(0);
v2 = seg(sind).pnt(1);
crdtocht1d(sind);
/* FIND ANGLE BETWEEN LINEAR SIDES */
tind = seg(sind).tri(0);
for(k=0;k<3;++k)
if (tri(tind).seg(k) == sind) break;
v0 = tri(tind).pnt(k);
dx0(0) = pnts(v2)(0)-pnts(v1)(0);
dx0(1) = pnts(v2)(1)-pnts(v1)(1);
length0 = dx0(0)*dx0(0) +dx0(1)*dx0(1);
dx1(0) = pnts(v0)(0)-pnts(v2)(0);
dx1(1) = pnts(v0)(1)-pnts(v2)(1);
length1 = dx1(0)*dx1(0) +dx1(1)*dx1(1);
dx2(0) = pnts(v1)(0)-pnts(v0)(0);
dx2(1) = pnts(v1)(1)-pnts(v0)(1);
length2 = dx2(0)*dx2(0) +dx2(1)*dx2(1);
basis::tri(log2p)->ptprobe1d(2,&ep(0),&dedpsi(0),-1.0,&cht(0,0),MXTM);
lengthept = dedpsi(0)*dedpsi(0) +dedpsi(1)*dedpsi(1);
ang1 = acos(-(dx0(0)*dx2(0) +dx0(1)*dx2(1))/sqrt(length0*length2));
curved1 = acos((dx0(0)*dedpsi(0) +dx0(1)*dedpsi(1))/sqrt(length0*lengthept));
basis::tri(log2p)->ptprobe1d(2,&ep(0),&dedpsi(0),1.0,&cht(0,0),MXTM);
lengthept = dedpsi(0)*dedpsi(0) +dedpsi(1)*dedpsi(1);
ang2 = acos(-(dx0(0)*dx1(0) +dx0(1)*dx1(1))/sqrt(length0*length1));
curved2 = acos((dx0(0)*dedpsi(0) +dx0(1)*dedpsi(1))/sqrt(length0*lengthept));
sum = gbl->curvature_sensitivity*(curved1/ang1 +curved2/ang2);
gbl->fltwk(v0) += sum*gbl->error_target*norm*pnt(v0).nnbor;
gbl->fltwk(v1) += sum*gbl->error_target*norm*pnt(v1).nnbor;
}
}
break;
}
}
for(i=0;i<npnt;++i) {
gbl->fltwk(i) = pow(gbl->fltwk(i)/(norm*pnt(i).nnbor*gbl->error_target),1./(basis::tri(log2p)->p()+1+ND));
lngth(i) /= gbl->fltwk(i);
}
/* AVOID HIGH ASPECT RATIOS */
int nsweep = 0;
do {
count = 0;
for(i=0;i<nseg;++i) {
v0 = seg(i).pnt(0);
v1 = seg(i).pnt(1);
ratio = lngth(v1)/lngth(v0);
if (ratio > 3.0) {
lngth(v1) = 2.5*lngth(v0);
++count;
}
else if (ratio < 0.333) {
lngth(v0) = 2.5*lngth(v1);
++count;
}
}
++nsweep;
*gbl->log << "#aspect ratio fixes " << nsweep << ' ' << count << std::endl;
} while(count > 0 && nsweep < 5);
return;
}
