DiagonalLinearFunction::DiagonalLinearFunction(Variable& x, const double diagonalElementValue,
const double vectorElementValue,
const bool useDefaultValue)
: NamedInstance("diagonal linear function")
, AbilitySet(PARTIAL_X)
, CoupledInputOutputSize(true)
, LinearFunction(x, x.getSize())
, _useDefaultValue(useDefaultValue), _defaultDiagonalValue(diagonalElementValue)
, _defaultbValue(vectorElementValue)
{
_d.resize(_dim);
_d.setConstant(diagonalElementValue);
buildA();
_b.resize(_dim);
_b.setConstant(vectorElementValue);
}
BaseOfSupport::BaseOfSupport(std::shared_ptr<StepController> stepController, const Eigen::MatrixXd& Q, const Eigen::MatrixXd& T, MIQPParameters miqpParams):
_stepController(stepController),
_miqpParams(miqpParams),
_Q(Q),
_T(T),
_Ab(Eigen::MatrixXd(4,2)),
_b(Eigen::VectorXd(4)),
_Cp(Eigen::MatrixXd(2, 6)),
_Ci(Eigen::MatrixXd(14,STATE_VECTOR_SIZE)),
_f(Eigen::VectorXd(14))
{
_A.resize(_Ci.rows()*miqpParams.N, T.cols()*miqpParams.N); _A.setZero();
_fbar.resize(_f.rows()*miqpParams.N); _fbar.setZero();
_B.resize(_Ci.rows()*miqpParams.N, _Q.cols()); _B.setZero();
_rhs.resize(miqpParams.N); _rhs.setZero();
// Build matrices of the bounding constraints when expressed in the terms of the state vector xi
buildAb();
buildCp(_miqpParams.cz, _miqpParams.g);
buildCi(_Ab, _Cp);
buildA(_Ci, _Q, _T);
buildB(_Ci, _Q);
_f.setZero();
}
void DiagonalLinearFunction::changeDiagonal(const double diagonalElementValue, const bool changeDefault)
{
doChangeDiagonal(diagonalElementValue, changeDefault);
buildA();
}
void DiagonalLinearFunction::changeDiagonal(const VectorXd& d)
{
doChangeDiagonal(d);
ocra_assert(_d.size() == _dim);
buildA();
}
void DiagonalLinearFunction::doUpdateInputSizeEnd()
{
buildA();
}
//////////////////////////////////////////////////////////////////////
// solve the Poisson equation with CG
//
// if heat is true, solve the heat equation, otherwise solve a
// generic Poisson equation
//////////////////////////////////////////////////////////////////////
void FLUID_3D_MIC::solvePoisson(FIELD_3D& x, FIELD_3D& b, unsigned char* skip, bool heat)
{
TIMER functionTimer(__FUNCTION__);
buildA(skip, heat);
buildMICPreconditioner();
multA(x,_residual);
TIMER residualTimer("Build residual");
int xi,yi,zi,
index = _slabSize + _xRes + 1;
for (zi = 1; zi < _zRes - 1; zi++, index += 2 * _xRes)
for (yi = 1; yi < _yRes - 1; yi++, index += 2)
for (xi = 1; xi < _xRes - 1; xi++, index++)
_residual[index] = b[index] - _residual[index];
zeroBoundary(_residual, skip);
residualTimer.stop();
applyMICPreconditioner(_z,_residual, _q);
_direction = _z;
// deltaNew = transpose(r) * r
Real deltaNew = _residual.dot(_z);
_solverEps = 1e-10;
// While deltaNew > (eps^2) * delta0
const Real eps = _solverEps;
Real maxR = 2.0f * eps;
//Real maxR = _residual.max();
int i = 0;
while ((i < _iterations) && (maxR > eps))
{
TIMER innerTimer("PCG inner loop");
// q = Ad
multA(_direction, _z);
// alpha = deltaNew / (transpose(d) * q)
TIMER dotTimer("PCG dot");
Real alpha = _direction.dot(_z);
dotTimer.stop();
// if alpha broke down, bail
if (fabs(alpha) > 0.0f)
alpha = deltaNew / alpha;
else
{
cout << __FILE__ << " " << __FUNCTION__ << " " << __LINE__ << " : " << endl;
cout << " PCG alpha broke down! Bailing." << endl;
break;
}
TIMER axpyTimer("PCG axpy");
// x = x + alpha * d
x.axpy(alpha, _direction);
// r = r - alpha * z
_residual.axpy(-alpha, _z);
axpyTimer.stop();
applyMICPreconditioner(_z,_residual, _q);
zeroBoundary(_q, skip);
Real oldMaxR = maxR;
_residual.setZeroBorder();
maxR = _residual.max();
// deltaOld = deltaNew
Real deltaOld = deltaNew;
// deltaNew = transpose(r) * r
TIMER dotTimer2("PCG dot");
deltaNew = _residual.dot(_z);
dotTimer2.stop();
// beta = deltaNew / deltaOld
TIMER directionTimer("PCG direction");
Real beta = deltaNew / deltaOld;
_direction *= beta;
_direction += _z;
zeroBoundary(_direction, skip);
directionTimer.stop();
// i = i + 1
i++;
// if we didn't make any relative progress, bail
Real relative = fabs((maxR - oldMaxR) / oldMaxR);
if (relative < 1e-7)
{
cout << __FILE__ << " " << __FUNCTION__ << " " << __LINE__ << " : " << endl;
cout << " Bailing! Residual before iteration: " << oldMaxR << " after: " << maxR << " relative change: " << relative << endl;
break;
}
//cout << " maxR: " << maxR << " alpha: " << alpha << " beta: " << beta << endl;
}
cout << " " << i << " iterations converged to inf norm: " << maxR << " two norm: " << _residual.flattened().norm2() << endl;
}
void SkelSmoothTerm::build()
{
buildA();
buildb();
}
double evqualitygrad(double *X, double* theta,const int *ik,const int *jk,int angle_num,int angle_index,int dim,int ndata)
{
/* build V,U,A */
double* matret=new double[dim*dim];
double* U1=new double[dim*dim];
double* U2=new double[dim*dim];
double* A=new double[ndata*dim];
double* matrot=new double[ndata*dim];
double *max_values = new double[ndata];
int *max_index = new int[ndata];
double dJ=0, tmp1, tmp2;
double* p_A;
double *p_Y;
/* find max of each row */
MatrixInitZeros(matret,dim,dim);
MatrixInitZeros(U1,dim,dim);
MatrixInitZeros(U2,dim,dim);
MatrixInitZeros(A,ndata,dim);
MatrixInitZeros(matrot,ndata,dim);
MatrixInitZeros(max_values,ndata,1);
MatrixInitZeros(max_index,ndata,1);
int i,j, ind = 0;
//getchar();
gradU(theta,angle_index,ik,jk,dim,matret);
/**/
//getchar();
build_Uab(theta,0,angle_index-1,ik,jk,dim,U1);
/**/
/**/
build_Uab(theta,angle_index+1,angle_num-1,ik,jk,dim,U2);
buildA(X,U1,matret,U2,A,ndata,dim);
/* rotate vecs according to current angles */
rotate_givens(X,theta,ik,jk,angle_num,ndata,dim,matrot);
p_Y=matrot;
MaxRowColumnAbsValue(p_Y,max_values,max_index,ndata,dim,1);
/* compute gradient */
ind = 0;
dJ=0;
for( i=0; i<ndata; i++ )
{ /* loop over all rows */
p_A=(double*)(A+i*dim);
p_Y=(double*)(matrot+i*dim);
for( j=0; j<dim; j++ )
{ /* loop over all columns */
tmp1 = p_A[j] * p_Y[j] / (max_values[i]*max_values[i]);
tmp2 = p_A[max_index[i]]*(p_Y[j]*p_Y[j])/(max_values[i]*max_values[i]*max_values[i]);
dJ += tmp1-tmp2;
}
}
dJ = 2*dJ/ndata/dim;
delete []max_values;
delete []max_index;
delete []matrot;
delete []matret;
delete []U2;
delete []U1;
delete []A;
return dJ;
};
void ARAPTerm::build()
{
initRotation();
buildA();
buildb();
}
