const Vector&
J2BeamFiber2d::getStress (void)
{
double G = 0.5*E/(1.0+nu);
sigma(0) = E*(Tepsilon(0)-epsPn[0]);
sigma(1) = G*(Tepsilon(1)-epsPn[1]);
static const double one3 = 1.0/3;
static const double two3 = 2.0*one3;
static const double root23 = sqrt(two3);
double xsi[2];
//xsi[0] = sigma(0) - two3*Hkin*1.5*epsPn[0];
//xsi[1] = sigma(1) - two3*Hkin*0.5*epsPn[1];
xsi[0] = sigma(0) - Hkin*epsPn[0];
xsi[1] = sigma(1) - one3*Hkin*epsPn[1];
double q = sqrt(two3*xsi[0]*xsi[0] + 2.0*xsi[1]*xsi[1]);
double F = q - root23*(sigmaY + Hiso*alphan);
if (F < -100*DBL_EPSILON) {
epsPn1[0] = epsPn[0];
epsPn1[1] = epsPn[1];
}
else {
// Solve for dg
double dg = 0.0;
static Vector R(3);
R(0) = 0.0; R(1) = 0.0; R(2) = F;
static Vector x(3);
x(0) = xsi[0]; x(1) = xsi[1]; x(2) = dg;
static Matrix J(3,3);
static Vector dx(3);
int iter = 0; int maxIter = 25;
while (iter < maxIter && R.Norm() > sigmaY*1.0e-14) {
iter++;
J(0,0) = 1.0 + dg*two3*(E+Hkin); J(0,1) = 0.0;
J(1,0) = 0.0; J(1,1) = 1.0 + dg*(2.0*G+two3*Hkin);
J(0,2) = two3*(E+Hkin)*x(0);
J(1,2) = (2.0*G+two3*Hkin)*x(1);
//J(2,0) = x(0)*two3/q; J(2,1) = x(1)*2.0/q;
J(2,0) = (1.0-two3*Hiso*dg)*x(0)*two3/q;
J(2,1) = (1.0-two3*Hiso*dg)*x(1)*2.0/q;
//J(2,2) = -root23*Hiso;
J(2,2) = -two3*Hiso*q;
J.Solve(R, dx);
x = x-dx;
dg = x(2);
dg_n1 = dg;
q = sqrt(two3*x(0)*x(0) + 2.0*x(1)*x(1));
R(0) = x(0) - xsi[0] + dg*two3*(E+Hkin)*x(0);
R(1) = x(1) - xsi[1] + dg*(2.0*G+two3*Hkin)*x(1);
R(2) = q - root23*(sigmaY + Hiso*(alphan+dg*root23*q));
}
if (iter == maxIter) {
//opserr << "J2BeamFiber2d::getStress -- maxIter reached " << R.Norm() << endln;
}
alphan1 = alphan + dg*root23*q;
epsPn1[0] = epsPn[0] + dg*two3*x(0);
epsPn1[1] = epsPn[1] + dg*2.0*x(1);
//sigma(0) = x(0) + two3*Hkin*1.5*epsPn1[0];
//sigma(1) = x(1) + two3*Hkin*0.5*epsPn1[1];
sigma(0) = x(0) + Hkin*epsPn1[0];
sigma(1) = x(1) + one3*Hkin*epsPn1[1];
}
return sigma;
}
Matrix operator*(double c1, const Matrix &c2)
{
return c2.multiply(c1);
}
Matrix operator*(const vec &c2, const Matrix &c1)
{
return c1.multiply(c2);
}
int
TFP_Bearing::kt3Drma(double *v, double *vp, double *Fr, double A, double *P, double *vpi) {
Vector vF (v, 8);
Vector vpF (vp, 8);
Vector FrF (Fr, 8);
Vector PF (P,4);
/*
opserr << "v: " << vF;
opserr << "vp: " << vpF;
opserr << "Fr: " << FrF;
opserr << "A: " << A << endln;
opserr << "P: " << PF;
*/
static double Ri[8];
static double R[8];
static double N[4];
static Matrix kcont(8,8);
static Matrix krot(8,8);
int cont = 0;
kthat.Zero();
kt.Zero();
ks.Zero();
Af.Zero();
kcont.Zero();
krot.Zero();
for (int i=0; i<4; i++)
N[i] = A;
for (int i=0; i<4; i++) {
int z=4+i;
Ri[i]=sqrt((r[i]-h[i])*(r[i]-h[i]) - v[z]*v[z]);
Ri[z]=sqrt((r[i]-h[i])*(r[i]-h[i]) - v[i]*v[i]);
d[i] = r[i] - sqrt(r[i]*r[i]) - sqrt(v[i]*v[i]+v[z]*v[z]);
N[i] = A + sqrt((P[0]-P[2])*(P[0]-P[2]) + (P[1]-P[3])*(P[1]-P[3])) *
sqrt(v[i]*v[i]+v[z]*v[z])/r[i];
R[i] = Ri[i];
R[z] = Ri[z];
}
double dh =0;
for (int i=0; i<4; i++) {
dh += d[i];
}
// R[0] = (Ri[0]*Ri[2])/(Ri[2]+fabs(v[2])*Ri[0]);
// R[1]=(Ri[1]*Ri[3])/(Ri[3]+fabs(v[3])*Ri[1]);
// R[4]=(Ri[4]*Ri[6])/(Ri[6]+fabs(v[4])*Ri[6]);
// R[5]=(Ri[5]*Ri[7])/(Ri[7]+fabs(v[5])*Ri[7]);
double PNorm = 0.0;
for (int i=0; i<4; i++) {
PNorm += P[i]*P[i];
}
PNorm = sqrt(PNorm);
N[0]=A+PNorm*(sqrt(v[0]*v[0]+v[4]*v[4])/r[0]+sqrt(v[2]*v[2]+v[6]*v[6])/r[2]);
N[1]=A+PNorm*(sqrt(v[1]*v[1]+v[5]*v[5])/r[1]+sqrt(v[3]*v[3]+v[7]*v[7])/r[3]);
for (int i=0; i<4; i++) {
int z=4+i;
// double vyield=0.01;
double qYield=mu[i]*N[i];
double k0=qYield/vyield;
//get trial shear forces of hysteretic component
double qTrialx = k0*(v[i] -vs[i]- vp[i]);
double qTrialy = k0*(v[z] -vs[z]- vp[z]);
// compute yield criterion of hysteretic component
double qTrialNorm = sqrt(qTrialx*qTrialx+qTrialy*qTrialy);
double Y = qTrialNorm - qYield;
// elastic step -> no updates for pastic displacements required
if (Y <= 0 ) {
// set tangent stiffnesses
ks(i,i) = k0 + N[i]/R[i];
ks(z,z) = k0 + N[i]/R[z];
vpi[i] = vp[i];
vpi[z] = vp[z];
// plastic step -> return mapping
} else {
// compute consistency parameters
double dGamma = Y/k0;
// update plastic displacements
vpi[i] = vp[i] + dGamma*qTrialx/qTrialNorm;
vpi[z] = vp[z] + dGamma*qTrialy/qTrialNorm;
// set tangent stiffnesses
double qTrialNorm3 = qTrialNorm*qTrialNorm*qTrialNorm;
ks(i,i) = qYield*k0*qTrialy*qTrialy/qTrialNorm3 + N[i]/R[i];
ks(i,z) = -qYield*k0*qTrialx*qTrialy/qTrialNorm3;
ks(z,i) = -qYield*k0*qTrialx*qTrialy/qTrialNorm3;
ks(z,z) = qYield*k0*qTrialx*qTrialx/qTrialNorm3 + N[i]/R[z];
}
//opserr << "ks: " << ks;
// restrainer contact stiffness
double vt=sqrt(v[i]*v[i]+v[z]*v[z]); //local displacment of surface
double rt=(dOut[i]-dIn[i])/2.0; //restrainer distance
double del=0.1;
if (vt>rt) {
cont=1;
double krim=k0*2;
// set restrainer stiffnesses
double vi2 = v[i]*v[i];
double vz2 = v[z]*v[z];
kcont(i,i) = krim*v[i]*v[i]/(vi2+vz2);
kcont(i,z) = krim*v[z]*v[i]/(vi2+vz2);
kcont(z,i) = krim*v[z]*v[i]/(vi2+vz2);
kcont(z,z) = krim*v[z]*v[z]/(vi2+vz2);
//force rotation matrix
double F=sqrt(Fr[i]*Fr[i]+Fr[z]*Fr[z]);
krot(i,i) = F* ((v[i]+del)/sqrt((v[i]+del)*(v[i]+del)+vz2) - (v[i]-del)/sqrt((v[i]-del)*(v[i]-del)+vz2));
krot(i,z) = F* (v[i]/sqrt(vi2+(v[z]+del)*(v[z]+del)) - v[i]/sqrt(vi2+(v[z]-del)*(v[z]-del)));
krot(z,i) = F* (v[z]/sqrt((v[i]+del)*(v[i]+del)+vz2) - v[z]/sqrt((v[i]-del)*(v[i]-del)+vz2));
krot(z,z) = F* ((v[z]+del)/sqrt(vi2+(v[z]+del)*v[z]+del) - (v[z]-del)/sqrt(vi2+(v[z]-del)*v[z]-del));
}
}
double del = 0.1;
for (int i=0; i<8; i++)
for (int j=0; j<8; j++)
ksrest(i,j)=kcont(i,j)+krot(i,j)/(del * 2.0);
// opserr << "ksrest: " << ksrest;
Af.Zero();
Af(0,4) = Ri[0];
Af(1,5) = Ri[1];
Af(2,0) = Ri[2]/(Ri[2]+Ri[3]);
Af(2,2) = -Ri[2]/(Ri[2]+Ri[3]);
Af(2,4) = -Ri[2]*(Ri[0]+Ri[3])/(Ri[2]+Ri[3]);
Af(2,5) = Ri[2]*(-Ri[1]+Ri[3])/(Ri[2]+Ri[3]);
Af(3,0) = Ri[3]/(Ri[2]+Ri[3]);
Af(3,2) = -Ri[3]/(Ri[2]+Ri[3]);
Af(3,4) = Ri[3]*(-Ri[0]+Ri[2])/(Ri[2]+Ri[3]);
Af(3,5) = Ri[3]*(-Ri[2]-Ri[1])/(Ri[2]+Ri[3]);
Af(4,6) = Ri[4];
Af(5,7) = Ri[5];
Af(6,1) = Ri[6]/(Ri[6]+Ri[7]);
Af(6,3) = -Ri[6]/(Ri[6]+Ri[7]);
Af(6,6) = -Ri[6]*(Ri[4]+Ri[7])/(Ri[6]+Ri[7]);
Af(6,7) = Ri[6]*(-Ri[5]+Ri[7])/(Ri[6]+Ri[7]);
Af(7,1) = Ri[7]/(Ri[6]+Ri[7]);
Af(7,3) = -Ri[7]/(Ri[6]+Ri[7]);
Af(7,6) = Ri[7]*(-Ri[4]+Ri[6])/(Ri[6]+Ri[7]);
Af(7,7) = Ri[7]*(-Ri[6]-Ri[5])/(Ri[6]+Ri[7]);
// opserr << "Af: " << Af;
// opserr << "ks: " << ks;
// opserr << "ksrest: " << ksrest;
static Matrix KsPlusKsrest(8,8);
KsPlusKsrest = ks;
KsPlusKsrest += ksrest;
KsPlusKsrest(0,2) = KsPlusKsrest(0,2) + N[0]/R[2];
KsPlusKsrest(1,3) = KsPlusKsrest(1,3) + N[1]/R[5];
KsPlusKsrest(4,6) = KsPlusKsrest(4,6) + N[4]/R[6];
KsPlusKsrest(5,7) = KsPlusKsrest(5,7) + N[5]/R[7];
kt.addMatrixTripleProduct(0.0, Af, KsPlusKsrest,1.0);
// opserr << "kt:" << kt;
static Matrix Kee(4,4);
for (int i=0; i<4; i++) {
for (int j=0; j<4; j++) {
kthat(i,j) = kt(i,j);
Kee(i,j) = kt(i+4, j+4);
kei(i,j) = kt(i+4, j);
}
}
Kee.Invert(kee);
kthat.addMatrixTripleProduct(1.0, kei, kee, -1.0);
// opserr << "kthat: " << kthat;
return cont;
}
Matrix operator+(const Matrix &c1, double c2)
{
return c1.add(c2);
}
/*! One possible formulation of the core GFO problem. Finds the contacts
forces that result in 0 object wrench and are as far as possible
from the edges of the friction cones. Assumes that at least one set
of contact forces that satisfy this criterion exist; see
contactForceExistence for that problem. See inner comments for exact
mathematical formulation. Not for standalone use; is called by the GFO
functions in the Grasp class.
*/
int
contactForceOptimization(Matrix &F, Matrix &N, Matrix &Q, Matrix &beta, double *objVal, Matrix * objectWrench = NULL)
{
// exact problem formulation
// unknowns: beta (contact forces)
// minimize sum * F * beta (crude measure of friction resistance abilities)
// subject to:
// Q * beta = 0 (0 resultant object wrench)
// sum_normal * beta = 1 (we are applying some contact forces)
// F * beta <= 0 (each individual forces inside friction cones)
// beta >= 0 (all forces must be positive)
// overall equality constraint:
// | Q | |beta| |0|
// | N | = |1|
//right hand of equality
Matrix right_hand = Matrix::ZEROES<Matrix>(Q.rows()+1, 1);
if(objectWrench)
{
assert(objectWrench->rows() == Q.rows());
for(int i = 0; i < Q.rows(); ++i)
right_hand.elem(i,0) = objectWrench->elem(i,0);
}
//a total of 10N of normal force
right_hand.elem(Q.rows(),0) = 1.0e7;
//left hand of equality
Matrix LeftHand(Q.rows() + 1, Q.cols());
LeftHand.copySubMatrix(0, 0, Q);
LeftHand.copySubMatrix(Q.rows(), 0, N);
//bounds: all variables greater than 0
Matrix lowerBounds(Matrix::ZEROES<Matrix>(beta.rows(),1));
Matrix upperBounds(Matrix::MAX_VECTOR(beta.rows()));
//objective: sum of F
Matrix FSum(1,F.rows());
FSum.setAllElements(1.0);
Matrix FObj(1,F.cols());
matrixMultiply(FSum, F, FObj);
/*
FILE *fp = fopen("gfo.txt","w");
fprintf(fp,"Left Hand:\n");
LeftHand.print(fp);
fprintf(fp,"right hand:\n");
right_hand.print(fp);
fprintf(fp,"friction inequality:\n");
F.print(fp);
fprintf(fp,"Objective:\n");
Q.print(fp);
fclose(fp);
*/
int result = LPSolver(FObj, LeftHand, right_hand, F, Matrix::ZEROES<Matrix>(F.rows(), 1),
lowerBounds, upperBounds,
beta, objVal);
return result;
}
/*! One possible version of the GFO problem.
Given a matrix of joint torques applied to the robot joints, this will check
if there exists a set of legal contact forces that balance them. If so, it
will compute the set of contact forces that adds up to the wrench of least
magnitude on the object.
For now, this output of this function is to set the computed contact forces
on each contact as dynamic forces, and also to accumulate the resulting
wrench on the object in its external wrench accumulator.
Return codes: 0 is success, >0 means finger slip, no legal contact forces
exist; <0 means error in computation
*/
int
Grasp::computeQuasistaticForces(const Matrix &robotTau)
{
//WARNING: for now, this ignores contacts on the palm. That might change in the future
//for now, if the hand is touching anything other then the object, abort
for (int c=0; c<hand->getNumChains(); c++) {
if ( hand->getChain(c)->getNumContacts(NULL) !=
hand->getChain(c)->getNumContacts(object) ) {
DBGA("Hand contacts not on object");
return 1;
}
}
std::list<Contact*> contacts;
std::list<Joint*> joints;
bool freeChainForces = false;
for(int c=0; c<hand->getNumChains(); c++) {
//for now, we look at all contacts
std::list<Contact*> chainContacts = hand->getChain(c)->getContacts(object);
contacts.insert(contacts.end(), chainContacts.begin(), chainContacts.end());
if (!chainContacts.empty()) {
std::list<Joint*> chainJoints = hand->getChain(c)->getJoints();
joints.insert(joints.end(), chainJoints.begin(), chainJoints.end());
} else {
//the chain has no contacts
//check if any joint forces are not 0
Matrix chainTau = hand->getChain(c)->jointTorquesVector(robotTau);
//torque units should be N * 1.0e6 * mm
if (chainTau.absMax() > 1.0e3) {
DBGA("Joint torque " << chainTau.absMax() << " on chain " << c
<< " with no contacts");
freeChainForces = true;
}
}
}
//if there are no contacts, nothing to compute!
if (contacts.empty()) return 0;
//assemble the joint forces matrix
Matrix tau((int)joints.size(), 1);
int jc; std::list<Joint*>::iterator jit;
for (jc=0, jit = joints.begin(); jit!=joints.end(); jc++,jit++) {
tau.elem(jc,0) = robotTau.elem( (*jit)->getNum(), 0 );
}
//if all the joint forces we care about are zero, do an early exit
//as zero contact forces are guaranteed to balance the chain
//we should probably be able to use a much larger threshold here, if
//units are what I think they are
if (tau.absMax() < 1.0e-3) {
return 0;
}
//if there are forces on chains with no contacts, we have no hope to balance them
if (freeChainForces) {
return 1;
}
Matrix J(contactJacobian(joints, contacts));
Matrix D(Contact::frictionForceBlockMatrix(contacts));
Matrix F(Contact::frictionConstraintsBlockMatrix(contacts));
Matrix R(Contact::localToWorldWrenchBlockMatrix(contacts));
//grasp map G = S*R*D
Matrix G(graspMapMatrix(R, D));
//left-hand equality matrix JTD = JTran * D
Matrix JTran(J.transposed());
Matrix JTD(JTran.rows(), D.cols());
matrixMultiply(JTran, D, JTD);
//matrix of zeroes for right-hand of friction inequality
Matrix zeroes(Matrix::ZEROES<Matrix>(F.rows(), 1));
//matrix of unknowns
Matrix c_beta(D.cols(), 1);
//bounds: all variables greater than 0
Matrix lowerBounds(Matrix::ZEROES<Matrix>(D.cols(),1));
Matrix upperBounds(Matrix::MAX_VECTOR(D.cols()));
/*
//test for sparse matrices
SparseMatrix JTDSparse(JTD.rows(), JTD.cols());
JTDSparse.copyMatrix(JTD);
DBGA("Dense matrix: \n" << JTD << "size: " << JTD.rows()*JTD.cols());
DBGA("Sparse matrix: \n" << JTDSparse << "size: " << JTDSparse.numElements() << "\n\n");
*/
//solve QP
double objVal;
int result = factorizedQPSolver(G, JTD, tau, F, zeroes, lowerBounds, upperBounds,
c_beta, &objVal);
if (result) {
if( result > 0) {
DBGP("Grasp: problem unfeasible");
} else {
DBGA("Grasp: QP solver error");
}
return result;
}
//retrieve contact wrenchs in local contact coordinate systems
Matrix cWrenches(D.rows(), 1);
matrixMultiply(D, c_beta, cWrenches);
DBGP("Contact wrenches:\n" << cWrenches);
//compute wrenches relative to object origin and expressed in world coordinates
Matrix objectWrenches(R.rows(), cWrenches.cols());
matrixMultiply(R, cWrenches, objectWrenches);
DBGP("Object wrenches:\n" << objectWrenches);
//display them on the contacts and accumulate them on the object
displayContactWrenches(&contacts, cWrenches);
accumulateAndDisplayObjectWrenches(&contacts, objectWrenches);
//simple sanity check: JT * c = tau
Matrix fCheck(tau.rows(), 1);
matrixMultiply(JTran, cWrenches, fCheck);
for (int j=0; j<tau.rows(); j++) {
//I am not sure this works well for universal and ball joints
double err = fabs(tau.elem(j, 0) - fCheck.elem(j,0));
//take error as a percentage of desired force, if force is non-zero
if ( fabs(tau.elem(j,0)) > 1.0e-2) {
err = err / fabs(tau.elem(j, 0));
} else {
//for zero desired torque, out of thin air we pull an error threshold of 1.0e2
//which is 0.1% of the normal range of torques at 1.0e6
if (err < 1.0e2) err = 0;
}
// 0.1% error is considered too much
if ( err > 1.0e-1) {
DBGA("Desired torque not obtained on joint " << j << ", error " << err <<
" out of " << fabs(tau.elem(j, 0)) );
return -1;
}
}
//complex sanity check: is object force same as QP optimization result?
//this is only expected to work if all contacts are on the same object
double* extWrench = object->getExtWrenchAcc();
vec3 force(extWrench[0], extWrench[1], extWrench[2]);
vec3 torque(extWrench[3], extWrench[4], extWrench[5]);
//take into account the scaling that has taken place
double wrenchError = objVal*1.0e-6 - (force.len_sq() + torque.len_sq()) * 1.0e6;
//units here are N * 1.0e-6; errors should be in the range on miliN
if (wrenchError > 1.0e3) {
DBGA("Wrench sanity check error: " << wrenchError);
return -1;
}
return 0;
}
void uniformMatrix4fv(GLint uniform, Matrix mat)
{
DirectX::XMFLOAT4X4 fMat;
XMStoreFloat4x4(&fMat, mat.getXMMatrix());
glUniformMatrix4fv(uniform, 1, GL_FALSE, &fMat.m[0][0]);
}
void SVDLinearSolver<TMatrix,TVector>::solve(Matrix& M, Vector& x, Vector& b)
{
#ifdef SOFA_DUMP_VISITOR_INFO
simulation::Visitor::printComment("SVD");
#endif
#ifdef DISPLAY_TIME
CTime * timer;
double time1 = (double) timer->getTime();
#endif
const bool printLog = this->f_printLog.getValue();
const bool verbose = f_verbose.getValue();
/// Convert the matrix and the right-hand vector to Eigen objects
Eigen::MatrixXd m(M.rowSize(),M.colSize());
Eigen::VectorXd rhs(M.rowSize());
for(unsigned i=0; i<M.rowSize(); i++ )
{
for( unsigned j=0; j<M.colSize(); j++ )
m(i,j) = M[i][j];
rhs(i) = b[i];
}
if(verbose)
{
serr << "SVDLinearSolver<TMatrix,TVector>::solve, Here is the matrix m:" << sendl << m << sendl;
}
/// Compute the SVD decomposition and the condition number
Eigen::JacobiSVD<Eigen::MatrixXd> svd(m, Eigen::ComputeThinU | Eigen::ComputeThinV);
f_conditionNumber.setValue( (Real)(svd.singularValues()(0) / svd.singularValues()(M.rowSize()-1)) );
if(printLog)
{
serr << "SVDLinearSolver<TMatrix,TVector>::solve, the singular values are:" << sendl << svd.singularValues() << sendl;
}
if(verbose)
{
serr << "Its left singular vectors are the columns of the thin U matrix:" << sendl << svd.matrixU() << sendl;
serr << "Its right singular vectors are the columns of the thin V matrix:" << sendl << svd.matrixV() << sendl;
}
/// Solve the equation system and copy the solution to the SOFA vector
// Eigen::VectorXd solution = svd.solve(rhs);
// for(unsigned i=0; i<M.rowSize(); i++ ){
// x[i] = solution(i);
// }
Eigen::VectorXd Ut_b = svd.matrixU().transpose() * rhs;
Eigen::VectorXd S_Ut_b(M.colSize());
for( unsigned i=0; i<M.colSize(); i++ ) /// product with the diagonal matrix, using the threshold for near-null values
{
if( svd.singularValues()[i] > f_minSingularValue.getValue() )
S_Ut_b[i] = Ut_b[i]/svd.singularValues()[i];
else
S_Ut_b[i] = (Real)0.0 ;
}
Eigen::VectorXd solution = svd.matrixV() * S_Ut_b;
for(unsigned i=0; i<M.rowSize(); i++ )
{
x[i] = (Real) solution(i);
}
if( printLog )
{
#ifdef DISPLAY_TIME
time1 = (double)(((double) timer->getTime() - time1) * timeStamp / (nb_iter-1));
std::cerr<<"SVDLinearSolver::solve, SVD = "<<time1<<std::endl;
#endif
serr << "SVDLinearSolver<TMatrix,TVector>::solve, rhs vector = " << sendl << rhs.transpose() << sendl;
serr << "SVDLinearSolver<TMatrix,TVector>::solve, solution = " << sendl << x << sendl;
serr << "SVDLinearSolver<TMatrix,TVector>::solve, verification, mx - b = " << sendl << (m * solution - rhs ).transpose() << sendl;
}
}
int main()
{
Cube c = Cube();
char* s = (char*)malloc(256*sizeof(char));
char* fun = (char*)malloc(256*sizeof(char));
float* params = (float*)malloc(10*sizeof(float));
int len=0;
char* word;
c.reset();
do {
cout<<"$ ";
cin.getline(s, 256, '\n');
if (!strcmp(s,"quit"))break;
if (strcmp(s,"r"))
{
word = strtok (s," ");
if (word == NULL) continue;
len = 0;
if (strcmp(word,"l"))
{
strcpy(fun, word);
}
while (len<9 && word != NULL)
{
word = strtok (NULL, " ");
if (word!=NULL)
{
params[len++] = atof(word);
}
}
}
if (!strcmp(fun, "reset")) {
c.reset();
} else if (!strcmp(fun, "translateX") && len>=1) {
c.translateX(params[0]);
} else if (!strcmp(fun, "translateY") && len>=1) {
c.translateY(params[0]);
} else if (!strcmp(fun, "translateZ") && len>=1) {
c.translateZ(params[0]);
} else if (!strcmp(fun, "rotateH") && len>=1) {
c.rotateH(params[0]);
} else if (!strcmp(fun, "rotateP") && len>=1) {
c.rotateP(params[0]);
} else if (!strcmp(fun, "rotateR") && len>=1) {
c.rotateR(params[0]);
}
else if (!strcmp(fun, "scale") && len>=1) {
c.scale(params[0]);
} else if (!strcmp(fun, "rotateHPR") && len>=3) {
c.rotateHPR(params[0], params[1], params[2]);
} else if (!strcmp(fun, "translateXYZ") && len>=3) {
c.translateXYZ(params[0], params[1], params[2]);
} else if (!strcmp(fun, "transformXYZHPRS") && len>=7) {
c.transformXYZHPRS(params[0], params[1], params[2],
params[3], params[4], params[5], params[6]);
} else if (!strcmp(fun, "toQuat")) {
c.mat->toQuat().print();
continue;
} else if (!strcmp(fun, "slerp_mat") && len>=10) {
Matrix m;
m.data[0][0] = params[0];
m.data[1][0] = params[1];
m.data[2][0] = params[2];
m.data[0][1] = params[3];
m.data[1][1] = params[4];
m.data[2][1] = params[5];
m.data[0][2] = params[6];
m.data[1][2] = params[7];
m.data[2][2] = params[8];
m.slerp(*(c.mat), params[9]).print();
continue;
} else if (!strcmp(fun, "slerp_quat") && len>=5) {
Quaternion q;
q.x = params[0];
q.y = params[1];
q.z = params[2];
q.w = params[3];
q.slerp(*(c.mat), params[4]).print();
continue;
} else if (strcmp(fun, "display")) {
cout << "Commands:\n" <<
"\treset\n" <<
"\tdisplay\n" <<
"\ttranslateX [x]\n" <<
"\ttranslateY [y]\n" <<
"\ttranslateZ [z]\n" <<
"\trotateH [h]\n" <<
"\trotateP [p]\n" <<
"\trotateR [r]\n" <<
"\tscale [s]\n" <<
"\trotateHPR [h] [p] [r]\n" <<
"\ttranslateXYZ [x] [y] [z]\n" <<
"\ttransformXYZHPRS [x] [y] [z] [h] [p] [r] [s]\n" <<
"\ttoQuat\n" <<
"\tslerp_mat [0,0] [1,0] [2,0] [0,1] [1,1] [2,1] " <<
"[0,2] [1,2] [2,2] B\n" <<
"\tslerp_quat [q1] [q2] [q3] [q0] B\n";
continue;
}
cout<<"\n";
c.print();
c.printMat();
cout<<"\n";
} while(1);
return 0;
}
int main(int argc, char **argv)
{
USING_NAMESPACE_ACADO
const double KKT_tol = 1e-6;
//READ THE DEMO LENGTHS & nBF FROM THE COMMAND LINE
std::deque<std::string> args(argv + 1, argv + argc + !argc);
const int nBF=atoi(args[0].c_str());
args.pop_front();
const int nD=(int)args.size();
int nS=0;
for(int i=0; i<nD;i++)
nS+=atoi(args[i].c_str());
//READ DATA
std::string path=DATA_PATH;
Matrix D = readFromFile((path+"demos.dat").c_str()); //d(:,0)=time, d(:,1)=x, d(:,2)=dx, d(:,3)=ddx;
Vector pI = readFromFile((path+"initial_guess.dat").c_str());
Matrix A = readFromFile((path+"inequality_constraint_matrix.dat").c_str());
Vector b = readFromFile((path+"inequality_constraint_vector.dat").c_str());
Matrix S = readFromFile((path+"scale_matrix.dat").c_str());
//RELEVANT INDEX SETS
std::vector<std::vector<int> > d_ind=getDemoInd(args);
std::vector<int> a_ind=getAInd(nBF,nD);
std::vector<int> b_ind=getBInd(nBF,nD);
std::vector<std::vector<int> > w_ind=getWInd(nBF,nD);
std::vector<int> r_ind=getRInd(nBF,nD);
std::vector<int> c_ind=getCInd(nBF,nD);
//PARAMETER & OBJECTIVE FUNCTION
Parameter p(2*nD+nBF*(2+nD)+1,1);
Matrix BM(nS,2*nD+nBF*(2+nD)+1); BM.setZero();
Expression B(BM);
double t,x,dx;
for (int d=0; d<nD; d++)
for(int s=0;s<(int)d_ind[d].size();s++)
{
t=D(d_ind[d][s],0);
x=D(d_ind[d][s],1);
dx=D(d_ind[d][s],2);
B(d_ind[d][s],a_ind[d])=x;
B(d_ind[d][s],b_ind[d])=dx;
for(int n=0;n<nBF;n++){
B(d_ind[d][s],w_ind[d][n])=(-0.5*(t-p(c_ind[n])*S(c_ind[n],c_ind[n])).getPowInt(2)/(p(r_ind[n])*p(r_ind[n])*S(r_ind[n],r_ind[n])*S(r_ind[n],r_ind[n]))).getExp();
// std::cout<<d<<std::endl;
//std::cout<< S(r_ind[d],r_ind[d])<<std::endl;
}
}
Expression f;
f<<B*S*p-D.getCol(3);
Expression ez(nS);
for (int i=0; i<nS; i++)
ez(i)=p(2*nD+nBF*(2+nD));
Vector e(nS); e.setAll(1.0);
Vector null(nS); null.setAll(0.0);
NLP nlp;
nlp.minimize(p(2*nD+nBF*(2+nD)));
nlp.subjectTo(f - ez <= null);
nlp.subjectTo(f + ez >= null);
//nlp.subjectTo(A*S*p <= b);
//ALGORITHM
ParameterEstimationAlgorithm algorithm(nlp);
VariablesGrid initial_guess(2*nD+nBF*(2+nD)+1,0.0,0.0,1 );
initial_guess.setVector( 0,S.getInverse()*pI );
algorithm.initializeParameters(initial_guess);
// OPTIONS
algorithm.set( KKT_TOLERANCE, KKT_tol);
algorithm.set( ABSOLUTE_TOLERANCE, 1e-4);
algorithm.set( PRINTLEVEL,HIGH);
algorithm.set( MAX_NUM_ITERATIONS, 2000 );
algorithm.set (PRINT_COPYRIGHT, NO);
// algorithm.set (PRINT_SCP_METHOD_PROFILE, YES);
algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN );
algorithm.set(GLOBALIZATION_STRATEGY, GS_LINESEARCH );
algorithm.set(LINESEARCH_TOLERANCE, 1e-2 );
algorithm.set(INFEASIBLE_QP_HANDLING,IQH_RELAX_L2);
algorithm.set(FEASIBILITY_CHECK,BT_TRUE);
// LOGGING
LogRecord logRecord( LOG_AT_EACH_ITERATION,(path+"log.dat").c_str(),PS_PLAIN);
logRecord << LOG_OBJECTIVE_VALUE;
algorithm << logRecord;
//SOLVING
double clock1 = clock();
algorithm.solve();
double clock2 = clock();
Vector solution;
algorithm.getParameters(solution);
// solution.print("optimal solution \n");
solution.printToFile((path+"solution.dat").c_str(),"",PS_PLAIN);
printf("\n computation time (ACADO) = %.16e \n", (clock2-clock1)/CLOCKS_PER_SEC);
return 0;
}
void LUMod
( Matrix<F>& A,
Permutation& P,
const Matrix<F>& u,
const Matrix<F>& v,
bool conjugate,
Base<F> tau )
{
DEBUG_CSE
typedef Base<F> Real;
const Int m = A.Height();
const Int n = A.Width();
const Int minDim = Min(m,n);
if( minDim != m )
LogicError("It is assumed that height(A) <= width(A)");
if( u.Height() != m || u.Width() != 1 )
LogicError("u is expected to be a conforming column vector");
if( v.Height() != n || v.Width() != 1 )
LogicError("v is expected to be a conforming column vector");
// w := inv(L) P u
auto w( u );
P.PermuteRows( w );
Trsv( LOWER, NORMAL, UNIT, A, w );
// Maintain an external vector for the temporary subdiagonal of U
Matrix<F> uSub;
Zeros( uSub, minDim-1, 1 );
// Reduce w to a multiple of e0
for( Int i=minDim-2; i>=0; --i )
{
// Decide if we should pivot the i'th and i+1'th rows of w
const F lambdaSub = A(i+1,i);
const F ups_ii = A(i,i);
const F omega_i = w(i);
const F omega_ip1 = w(i+1);
const Real rightTerm = Abs(lambdaSub*omega_i+omega_ip1);
const bool pivot = ( Abs(omega_i) < tau*rightTerm );
const Range<Int> indi( i, i+1 ),
indip1( i+1, i+2 ),
indB( i+2, m ),
indR( i+1, n );
auto lBi = A( indB, indi );
auto lBip1 = A( indB, indip1 );
auto uiR = A( indi, indR );
auto uip1R = A( indip1, indR );
if( pivot )
{
// P := P_i P
P.Swap( i, i+1 );
// Simultaneously perform
// U := P_i U and
// L := P_i L P_i^T
//
// Then update
// L := L T_{i,L}^{-1},
// U := T_{i,L} U,
// w := T_{i,L} P_i w,
// where T_{i,L} is the Gauss transform which zeros (P_i w)_{i+1}.
//
// More succinctly,
// gamma := w(i) / w(i+1),
// w(i) := w(i+1),
// w(i+1) := 0,
// L(:,i) += gamma L(:,i+1),
// U(i+1,:) -= gamma U(i,:).
const F gamma = omega_i / omega_ip1;
const F lambda_ii = F(1) + gamma*lambdaSub;
A(i, i) = gamma;
A(i+1,i) = 0;
auto lBiCopy = lBi;
Swap( NORMAL, lBi, lBip1 );
Axpy( gamma, lBiCopy, lBi );
auto uip1RCopy = uip1R;
RowSwap( A, i, i+1 );
Axpy( -gamma, uip1RCopy, uip1R );
// Force L back to *unit* lower-triangular form via the transform
// L := L T_{i,U}^{-1} D^{-1},
// where D is diagonal and responsible for forcing L(i,i) and
// L(i+1,i+1) back to 1. The effect on L is:
// eta := L(i,i+1)/L(i,i),
// L(:,i+1) -= eta L(:,i),
// delta_i := L(i,i),
// delta_ip1 := L(i+1,i+1),
// L(:,i) /= delta_i,
// L(:,i+1) /= delta_ip1,
// while the effect on U is
// U(i,:) += eta U(i+1,:)
// U(i,:) *= delta_i,
// U(i+1,:) *= delta_{i+1},
// and the effect on w is
// w(i) *= delta_i.
const F eta = lambdaSub/lambda_ii;
const F delta_i = lambda_ii;
const F delta_ip1 = F(1) - eta*gamma;
Axpy( -eta, lBi, lBip1 );
A(i+1,i) = gamma/delta_i;
lBi *= F(1)/delta_i;
lBip1 *= F(1)/delta_ip1;
A(i,i) = eta*ups_ii*delta_i;
Axpy( eta, uip1R, uiR );
uiR *= delta_i;
uip1R *= delta_ip1;
uSub(i) = ups_ii*delta_ip1;
// Finally set w(i)
w(i) = omega_ip1*delta_i;
}
else
{
// Update
// L := L T_{i,L}^{-1},
// U := T_{i,L} U,
// w := T_{i,L} w,
// where T_{i,L} is the Gauss transform which zeros w_{i+1}.
//
// More succinctly,
// gamma := w(i+1) / w(i),
// L(:,i) += gamma L(:,i+1),
// U(i+1,:) -= gamma U(i,:),
// w(i+1) := 0.
const F gamma = omega_ip1 / omega_i;
A(i+1,i) += gamma;
Axpy( gamma, lBip1, lBi );
Axpy( -gamma, uiR, uip1R );
uSub(i) = -gamma*ups_ii;
}
}
// Add the modified w v' into U
{
auto a0 = A( IR(0), ALL );
const F omega_0 = w(0);
Matrix<F> vTrans;
Transpose( v, vTrans, conjugate );
Axpy( omega_0, vTrans, a0 );
}
// Transform U from upper-Hessenberg to upper-triangular form
for( Int i=0; i<minDim-1; ++i )
{
// Decide if we should pivot the i'th and i+1'th rows U
const F lambdaSub = A(i+1,i);
const F ups_ii = A(i,i);
const F ups_ip1i = uSub(i);
const Real rightTerm = Abs(lambdaSub*ups_ii+ups_ip1i);
const bool pivot = ( Abs(ups_ii) < tau*rightTerm );
const Range<Int> indi( i, i+1 ),
indip1( i+1, i+2 ),
indB( i+2, m ),
indR( i+1, n );
auto lBi = A( indB, indi );
auto lBip1 = A( indB, indip1 );
auto uiR = A( indi, indR );
auto uip1R = A( indip1, indR );
if( pivot )
{
// P := P_i P
P.Swap( i, i+1 );
// Simultaneously perform
// U := P_i U and
// L := P_i L P_i^T
//
// Then update
// L := L T_{i,L}^{-1},
// U := T_{i,L} U,
// where T_{i,L} is the Gauss transform which zeros U(i+1,i).
//
// More succinctly,
// gamma := U(i+1,i) / U(i,i),
// L(:,i) += gamma L(:,i+1),
// U(i+1,:) -= gamma U(i,:).
const F gamma = ups_ii / ups_ip1i;
const F lambda_ii = F(1) + gamma*lambdaSub;
A(i+1,i) = ups_ip1i;
A(i, i) = gamma;
auto lBiCopy = lBi;
Swap( NORMAL, lBi, lBip1 );
Axpy( gamma, lBiCopy, lBi );
auto uip1RCopy = uip1R;
RowSwap( A, i, i+1 );
Axpy( -gamma, uip1RCopy, uip1R );
// Force L back to *unit* lower-triangular form via the transform
// L := L T_{i,U}^{-1} D^{-1},
// where D is diagonal and responsible for forcing L(i,i) and
// L(i+1,i+1) back to 1. The effect on L is:
// eta := L(i,i+1)/L(i,i),
// L(:,i+1) -= eta L(:,i),
// delta_i := L(i,i),
// delta_ip1 := L(i+1,i+1),
// L(:,i) /= delta_i,
// L(:,i+1) /= delta_ip1,
// while the effect on U is
// U(i,:) += eta U(i+1,:)
// U(i,:) *= delta_i,
// U(i+1,:) *= delta_{i+1}.
const F eta = lambdaSub/lambda_ii;
const F delta_i = lambda_ii;
const F delta_ip1 = F(1) - eta*gamma;
Axpy( -eta, lBi, lBip1 );
A(i+1,i) = gamma/delta_i;
lBi *= F(1)/delta_i;
lBip1 *= F(1)/delta_ip1;
A(i,i) = ups_ip1i*delta_i;
Axpy( eta, uip1R, uiR );
uiR *= delta_i;
uip1R *= delta_ip1;
}
else
{
// Update
// L := L T_{i,L}^{-1},
// U := T_{i,L} U,
// where T_{i,L} is the Gauss transform which zeros U(i+1,i).
//
// More succinctly,
// gamma := U(i+1,i)/ U(i,i),
// L(:,i) += gamma L(:,i+1),
// U(i+1,:) -= gamma U(i,:).
const F gamma = ups_ip1i / ups_ii;
A(i+1,i) += gamma;
Axpy( gamma, lBip1, lBi );
Axpy( -gamma, uiR, uip1R );
}
}
}
int
J2BeamFiber2d::commitSensitivity(const Vector &depsdh, int gradIndex, int numGrads)
{
if (SHVs == 0) {
SHVs = new Matrix(3,numGrads);
}
if (gradIndex >= SHVs->noCols()) {
//opserr << gradIndex << ' ' << SHVs->noCols() << endln;
return 0;
}
//return 0;
double dEdh = 0.0;
double dsigmaYdh = 0.0;
double dHkindh = 0.0;
double dHisodh = 0.0;
double dGdh = 0.0;
if (parameterID == 1) { // E
dEdh = 1.0;
dGdh = 0.5/(1.0+nu);
}
if (parameterID == 2) { // nu
dGdh = -0.5*E/(1.0 + 2.0*nu + nu*nu);
}
if (parameterID == 5) {
dsigmaYdh = 1.0;
}
if (parameterID == 6) {
dHkindh = 1.0;
}
if (parameterID == 7) {
dHisodh = 1.0;
}
double G = 0.5*E/(1.0+nu);
double depsPdh[2]; depsPdh[0] = 0.0; depsPdh[1] = 0.0;
double dalphadh = 0.0;
if (SHVs != 0) {
depsPdh[0] = (*SHVs)(0,gradIndex);
depsPdh[1] = (*SHVs)(1,gradIndex);
dalphadh = (*SHVs)(2,gradIndex);
}
static const double one3 = 1.0/3;
static const double two3 = 2.0*one3;
static const double root23 = sqrt(two3);
double xsi[2];
xsi[0] = E*(Tepsilon(0)-epsPn1[0]) - Hkin*epsPn1[0];
xsi[1] = G*(Tepsilon(1)-epsPn1[1]) - one3*Hkin*epsPn1[1];
double q = sqrt(two3*xsi[0]*xsi[0] + 2.0*xsi[1]*xsi[1]);
double F = q - root23*(sigmaY + Hiso*alphan1);
if (F <= -100*DBL_EPSILON) {
// Do nothing
}
else {
static Matrix J(3,3);
static Vector b(3);
static Vector dx(3);
double dg = dg_n1;
J(0,0) = 1.0 + dg*two3*(E+Hkin); J(0,1) = 0.0;
J(1,0) = 0.0; J(1,1) = 1.0 + dg*(2.0*G+two3*Hkin);
J(0,2) = two3*(E+Hkin)*xsi[0];
J(1,2) = (2.0*G+two3*Hkin)*xsi[1];
//J(2,0) = xsi[0]*two3/q; J(2,1) = xsi[1]*2.0/q;
J(2,0) = (1.0-two3*Hiso*dg)*xsi[0]*two3/q;
J(2,1) = (1.0-two3*Hiso*dg)*xsi[1]*2.0/q;
//J(2,2) = -root23*Hiso;
J(2,2) = -two3*Hiso*q;
b(0) = E*depsdh(0) + dEdh*Tepsilon(0) - (E+ Hkin)*depsPdh[0] - (dEdh+ dHkindh)*epsPn1[0];
b(1) = G*depsdh(1) + dGdh*Tepsilon(1) - (G+one3*Hkin)*depsPdh[1] - (dGdh+one3*dHkindh)*epsPn1[1];
b(2) = root23*(dsigmaYdh + dHisodh*alphan1 + Hiso*dalphadh);
J.Solve(b, dx);
dalphadh += dx(2)*root23*q + dg*root23*(xsi[0]*two3*dx(0)+xsi[1]*2.0*dx(1))/q;
depsPdh[0] += dx(2)*two3*xsi[0] + dg*two3*dx(0);
depsPdh[1] += dx(2)* 2.0*xsi[1] + dg* 2.0*dx(1);
(*SHVs)(0,gradIndex) = depsPdh[0];
(*SHVs)(1,gradIndex) = depsPdh[1];
(*SHVs)(2,gradIndex) = dalphadh;
}
return 0;
}
const Vector&
J2BeamFiber2d::getStressSensitivity(int gradIndex, bool conditional)
{
static Vector sigma(2);
sigma(0) = 0.0;
sigma(1) = 0.0;
double dEdh = 0.0;
double dsigmaYdh = 0.0;
double dHkindh = 0.0;
double dHisodh = 0.0;
double dGdh = 0.0;
if (parameterID == 1) { // E
dEdh = 1.0;
dGdh = 0.5/(1.0+nu);
}
if (parameterID == 2) { // nu
dGdh = -0.5*E/(1.0 + 2.0*nu + nu*nu);
}
if (parameterID == 5) {
dsigmaYdh = 1.0;
}
if (parameterID == 6) {
dHkindh = 1.0;
}
if (parameterID == 7) {
dHisodh = 1.0;
}
double G = 0.5*E/(1.0+nu);
double depsPdh[2]; depsPdh[0] = 0.0; depsPdh[1] = 0.0;
double dalphadh = 0.0;
if (SHVs != 0) {
depsPdh[0] = (*SHVs)(0,gradIndex);
depsPdh[1] = (*SHVs)(1,gradIndex);
dalphadh = (*SHVs)(2,gradIndex);
}
static const double one3 = 1.0/3;
static const double two3 = 2.0*one3;
static const double root23 = sqrt(two3);
double xsi[2];
xsi[0] = E*(Tepsilon(0)-epsPn1[0]) - Hkin*epsPn1[0];
xsi[1] = G*(Tepsilon(1)-epsPn1[1]) - one3*Hkin*epsPn1[1];
double q = sqrt(two3*xsi[0]*xsi[0] + 2.0*xsi[1]*xsi[1]);
double F = q - root23*(sigmaY + Hiso*alphan1);
if (F <= -100*DBL_EPSILON) {
sigma(0) = dEdh*(Tepsilon(0)-epsPn1[0]) - E*depsPdh[0];
sigma(1) = dGdh*(Tepsilon(1)-epsPn1[1]) - G*depsPdh[1];
}
else {
static Matrix J(3,3);
static Vector b(3);
static Vector dx(3);
double dg = dg_n1;
J(0,0) = 1.0 + dg*two3*(E+Hkin); J(0,1) = 0.0;
J(1,0) = 0.0; J(1,1) = 1.0 + dg*(2.0*G+two3*Hkin);
J(0,2) = two3*(E+Hkin)*xsi[0];
J(1,2) = (2.0*G+two3*Hkin)*xsi[1];
//J(2,0) = xsi[0]*two3/q; J(2,1) = xsi[1]*2.0/q;
J(2,0) = (1.0-two3*Hiso*dg)*xsi[0]*two3/q;
J(2,1) = (1.0-two3*Hiso*dg)*xsi[1]*2.0/q;
//J(2,2) = -root23*Hiso;
J(2,2) = -two3*Hiso*q;
b(0) = dEdh*Tepsilon(0) - (E+ Hkin)*depsPdh[0] - (dEdh+ dHkindh)*epsPn1[0];
b(1) = dGdh*Tepsilon(1) - (G+one3*Hkin)*depsPdh[1] - (dGdh+one3*dHkindh)*epsPn1[1];
b(2) = root23*(dsigmaYdh + dHisodh*alphan1 + Hiso*dalphadh);
J.Solve(b, dx);
depsPdh[0] += dx(2)*two3*xsi[0] + dg*two3*dx(0);
depsPdh[1] += dx(2)* 2.0*xsi[1] + dg* 2.0*dx(1);
sigma(0) = dx(0) + Hkin*depsPdh[0] + dHkindh*epsPn1[0];
sigma(1) = dx(1) + one3*Hkin*depsPdh[1] + one3*dHkindh*epsPn1[1];
}
return sigma;
}
int main(int argc, char* argv[])
{
if(argc < 2)
{
cout << "Please provide a control file name.\n";
return -1;
}
string confile = argv[1];
string inp, outp, outdg, jacs, dum, anglestr, solver;
double suprad, tol;
int nmarks; // number of boundary markers to read
int nrbfsteps, maxiter;
vector<double> centre(2);
Matrix<int> n_rot;
ifstream conf(confile);
conf >> dum; conf >> inp;
conf >> dum; conf >> outp;
conf >> dum; conf >> jacs;
conf >> dum; conf >> anglestr;
conf >> dum; conf >> centre[0] >> centre[1];
conf >> dum; conf >> nmarks;
conf >> dum;
n_rot.setup(nmarks,1);
for(int i = 0; i < nmarks; i++)
conf >> n_rot(i);
conf >> dum; conf >> suprad;
conf >> dum; conf >> nrbfsteps;
conf >> dum; conf >> solver;
conf >> dum; conf >> tol;
conf >> dum; conf >> maxiter;
conf.close();
double angle = stod(anglestr)*PI/180.0; // convert to radians
cout << "support radius is " << suprad << endl;
cout << "Centre is " << centre[0] << " " << centre[1] << endl;
// read mesh
UMesh2d m;
m.readGmsh2(inp,2);
// create a vector do distinguish boundary points from interior points
Matrix<int> flags(m.gnpoin(),1);
flags.zeros();
for(int i = 0; i < m.gnface(); i++)
for(int j = 0; j < m.gnnofa(); j++)
flags(m.gbface(i,j)) = 1;
//calculate boundary displacement
MRotation2d rot(&m, angle, centre[0], centre[1], n_rot);
Matrix<double> bc = rot.rhsvect_rotate();
// get number of interior points and boundary points
int n_bpoin=0, n_inpoin=0;
for(int i = 0; i < m.gnpoin(); i++)
n_bpoin += flags(i);
n_inpoin = m.gnpoin() - n_bpoin;
// Split interior data and boundary data
Matrix<double> inpoints(n_inpoin, m.gndim());
Matrix<double> bpoints(n_bpoin, m.gndim());
Matrix<double> bcb(n_bpoin, m.gndim());
int k = 0;
for(int i = 0; i < flags.rows(); i++)
if(flags(i) == 0)
{
for(int j = 0; j < m.gndim(); j++)
inpoints(k,j) = m.gcoords(i,j);
k++;
}
k = 0;
for(int i = 0; i < flags.rows(); i++)
if(flags(i) == 1)
{
for(int j = 0; j < m.gndim(); j++){
bpoints(k,j) = m.gcoords(i,j);
bcb(k,j) = bc.get(i,j);
}
k++;
}
// carry out DG mapping procedure
RBFmove d;
d.setup(&inpoints, &bpoints, &bcb, 2, suprad, nrbfsteps, tol, maxiter,solver);
d.move();
inpoints = d.getInteriorPoints();
bpoints = d.getBoundaryPoints();
// create a coords matrix with same point numbering as initial matrix and return it
Matrix<double> newcoords(m.gnpoin(),m.gndim());
int a = 0, b = 0; k = 0;
for(int i = 0; i < flags.rows(); i++)
{
if(flags(i) == 0)
{
for(int dim = 0; dim < m.gndim(); dim++)
newcoords(k,dim) = inpoints(a,dim);
k++;
a++;
}
else
{
for(int dim = 0; dim < m.gndim(); dim++)
newcoords(k,dim) = bpoints.get(b,dim);
k++;
b++;
}
}
m.setcoords(&newcoords);
m.writeGmsh2(outp);
m.compute_jacobians();
cout << "Checking jacobians\n";
ofstream ojac(jacs);
m.detect_negative_jacobians(ojac);
ojac.close();
cout << "Done.\n" << endl;
return 0;
}
static Matrix<float, 3, 24> wireframe(const float scale) {
Matrix<float, 3, 24> m;
m.col(0) << -1, -1, -1;
m.col(1) << -1, -1, +1;
m.col(2) << -1, -1, -1;
m.col(3) << -1, +1, -1;
m.col(4) << -1, -1, -1;
m.col(5) << +1, -1, -1;
m.col(6) << -1, -1, +1;
m.col(7) << -1, +1, +1;
m.col(8) << -1, -1, +1;
m.col(9) << +1, -1, +1;
m.col(10) << -1, +1, -1;
m.col(11) << -1, +1, +1;
m.col(12) << -1, +1, -1;
m.col(13) << +1, +1, -1;
m.col(14) << -1, +1, +1;
m.col(15) << +1, +1, +1;
m.col(16) << +1, -1, -1;
m.col(17) << +1, -1, +1;
m.col(18) << +1, -1, -1;
m.col(19) << +1, +1, -1;
m.col(20) << +1, -1, +1;
m.col(21) << +1, +1, +1;
m.col(22) << +1, +1, -1;
m.col(23) << +1, +1, +1;
m *= scale;
return m;
}
LRESULT WINAPI WndProc(HWND hWnd, UINT uMsg, WPARAM wParam, LPARAM lParam){
LRESULT lRet = 1;
int wmId, wmEvent;
PAINTSTRUCT ps;
HDC hdc;
switch (uMsg){
case WM_CREATE:
break;
case WM_PAINT:
ValidateRect(g_wnd, NULL);
break;
case WM_DESTROY:
PostQuitMessage(0);
break;
case WM_CHAR:
break;
case WM_LBUTTONDOWN:
{
POINT ptMouse;
GetCursorPos(&ptMouse);
ScreenToClient(g_wnd, &ptMouse);
g_ptLastPoint = ptMouse;
}
break;
case WM_RBUTTONDOWN:
{
GetCursorPos(&g_ptLastPoint);
ScreenToClient(g_wnd, &g_ptLastPoint);
break;
}
case WM_MOUSEMOVE:
{
switch (wParam)
{
case MK_LBUTTON:
{
}
break;
case MK_RBUTTON:
{
POINT pt;
pt.x = LOWORD(lParam);
pt.y = HIWORD(lParam);
g_camera->SetRotAngleDelta((pt.y - g_ptLastPoint.y) / 150.0f, (pt.x - g_ptLastPoint.x) / 150.0f, 0.0f);
g_ptLastPoint = pt;
}
default:
break;
}
break;
}
case WM_KEYDOWN:
{
Vector *vcDirc = new Vector();
Vector *vcUp = new Vector();
Vector *vcRight = new Vector();
g_camera->GetDirection(vcDirc, vcUp, vcRight);
switch (wParam)
{
case VK_A:
{
g_camera->SetMoveDirection(*vcRight);
g_camera->SetMoveDelta(-20.0f);
g_camera->Update();
Matrix matView;
g_camera->GetViewMatrix(&matView);
break;
}
case VK_D:
{
Vector vcPosCamera;
g_camera->SetMoveDirection(*vcRight);
g_camera->SetMoveDelta(20.0f);
g_camera->Update();
Matrix matView;
g_camera->GetViewMatrix(&matView);
break;
}
case VK_W:
{
g_camera->SetMoveDirection(*vcDirc);
g_camera->SetMoveDelta(20.0f);
g_camera->Update();
Matrix matView;
g_camera->GetViewMatrix(&matView);
break;
}
case VK_S:
{
g_camera->SetMoveDirection(*vcDirc);
g_camera->SetMoveDelta(-20.0f);
g_camera->Update();
Matrix matView;
g_camera->GetViewMatrix(&matView);
break;
}
case VK_Q:
{
g_camera->SetMoveDirection(*vcUp);
g_camera->SetMoveDelta(20.0f);
g_camera->Update();
Matrix matView;
g_camera->GetViewMatrix(&matView);
break;
}
case VK_E:
{
g_camera->SetMoveDirection(*vcUp);
g_camera->SetMoveDelta(-20.0f);
g_camera->Update();
Matrix matView;
g_camera->GetViewMatrix(&matView);
break;
}
default:
break;
}
g_camera->Update();
Matrix viewMat;
viewMat.Identity();
g_camera->GetViewMatrix(&viewMat);
}
break;
default:
lRet = DefWindowProc(hWnd, uMsg, wParam, lParam);
break;
}
return lRet;
}
RayCastModelHit Model::castRay(const Vec3& origin,
const Vec3& dir,
const Matrix& model_transform)
{
RayCastModelHit hit;
hit.m_is_hit = false;
if (!isReady())
{
return hit;
}
Matrix inv = model_transform;
inv.inverse();
Vec3 local_origin = inv.multiplyPosition(origin);
Vec3 local_dir = static_cast<Vec3>(inv * Vec4(dir.x, dir.y, dir.z, 0));
const Array<Vec3>& vertices = m_vertices;
const Array<int32_t>& indices = m_indices;
int vertex_offset = 0;
for (int mesh_index = 0; mesh_index < m_meshes.size(); ++mesh_index)
{
int indices_end = m_meshes[mesh_index].getIndicesOffset() +
m_meshes[mesh_index].getIndexCount();
for (int i = m_meshes[mesh_index].getIndicesOffset(); i < indices_end;
i += 3)
{
Vec3 p0 = vertices[vertex_offset + indices[i]];
Vec3 p1 = vertices[vertex_offset + indices[i + 1]];
Vec3 p2 = vertices[vertex_offset + indices[i + 2]];
Vec3 normal = crossProduct(p1 - p0, p2 - p0);
float q = dotProduct(normal, local_dir);
if (q == 0)
{
continue;
}
float d = -dotProduct(normal, p0);
float t = -(dotProduct(normal, local_origin) + d) / q;
if (t < 0)
{
continue;
}
Vec3 hit_point = local_origin + local_dir * t;
Vec3 edge0 = p1 - p0;
Vec3 VP0 = hit_point - p0;
if (dotProduct(normal, crossProduct(edge0, VP0)) < 0)
{
continue;
}
Vec3 edge1 = p2 - p1;
Vec3 VP1 = hit_point - p1;
if (dotProduct(normal, crossProduct(edge1, VP1)) < 0)
{
continue;
}
Vec3 edge2 = p0 - p2;
Vec3 VP2 = hit_point - p2;
if (dotProduct(normal, crossProduct(edge2, VP2)) < 0)
{
continue;
}
if (!hit.m_is_hit || hit.m_t > t)
{
hit.m_is_hit = true;
hit.m_t = t;
hit.m_mesh = &m_meshes[mesh_index];
}
}
vertex_offset += m_meshes[mesh_index].getAttributeArraySize() /
m_meshes[mesh_index].getVertexDefinition().getStride();
}
hit.m_origin = origin;
hit.m_dir = dir;
return hit;
}
/*! One possible formulation of the core GFO problem. Finds the joint
forces that result in 0 object wrench such that contact forces are as
far as possible from the edges of the friction cones. Assumes that at
least one set of contact forces that satisfy this criterion exist; see
contactForceExistence for that problem. See inner comments for exact
mathematical formulation. Not for standalone use; is called by the GFO
functions in the Grasp class.
*/
int
graspForceOptimization(Matrix &JTD, Matrix &D, Matrix &F, Matrix &Q,
Matrix &beta, Matrix &tau, double *objVal, Matrix * objectWrench = NULL)
{
// exact problem formulation
// unknowns: [beta tau] (contact forces and joint forces)
// minimize [sum] [F 0] [beta tau] (sort of as far inside the friction cone as possible, not ideal)
// subject to:
// [JTD -I] * [beta tau] = 0 (contact forces balance joint forces)
// [Q 0]* [beta tau] = 0 (0 resultant object wrench)
// [0 sum] * [beta tau] = 1 (we are applying some joint forces)
// [F 0] [beta tau] <= 0 (all forces inside friction cones)
// [beta tau] >= 0 (all forces must be positive)
// overall equality constraint:
// | JTD -I | | beta | |0|
// | Q 0 | | tau | = |0|
// | 0 sum| |1|
Matrix beta_tau(beta.rows() + tau.rows(), 1);
int numJoints = tau.rows();
//right-hand side of equality constraint
Matrix right_hand( JTD.rows() + Q.rows() + 1, 1);
right_hand.setAllElements(0.0);
//actually, we use 1.0e7 here as units are in N * 1.0e-6 * mm
//so we want a total joint torque of 10 N mm
right_hand.elem( right_hand.rows()-1, 0) = 1.0e7;
if(objectWrench)
for (int i = 0; i < 6; ++i)
right_hand.elem(JTD.rows() + i,0) = objectWrench->elem(i,0);
std::cout << "QRows " << Q.rows() <<" QCols " << Q.cols() << std::endl;
//left-hand side of equality constraint
Matrix LeftHand( JTD.rows() + Q.rows() + 1, D.cols() + numJoints);
LeftHand.setAllElements(0.0);
LeftHand.copySubMatrix(0, 0, JTD);
LeftHand.copySubMatrix(0, D.cols(), Matrix::NEGEYE(numJoints, numJoints) );
LeftHand.copySubMatrix(JTD.rows(), 0, Q);
for (int i=0; i<numJoints; i++) {
LeftHand.elem( JTD.rows() + Q.rows(), D.cols() + i) = 1.0;
}
//objective matrix
//matrix F padded with zeroes for tau
//will also serve as the left hand side of the inequality matrix
Matrix FO(F.rows(), F.cols() + numJoints);
FO.setAllElements(0.0);
FO.copySubMatrix(0, 0, F);
//summing matrix and objective matrix
Matrix FSum(1, F.rows());
FSum.setAllElements(1.0);
Matrix FObj(1, FO.cols());
matrixMultiply(FSum, FO, FObj);
//bounds: all variables greater than 0
Matrix lowerBounds(Matrix::ZEROES<Matrix>(beta_tau.rows(),1));
Matrix upperBounds(Matrix::MAX_VECTOR(beta_tau.rows()));
//right-hand side of inequality matrix
Matrix inEqZeroes(FO.rows(), 1);
inEqZeroes.setAllElements(0.0);
/*
FILE *fp = fopen("gfo.txt","w");
fprintf(fp,"left hand:\n");
LeftHand.print(fp);
fprintf(fp,"right hand:\n");
right_hand.print(fp);
fprintf(fp,"friction inequality:\n");
FO.print(fp);
fprintf(fp,"Objective:\n");
FObj.print(fp);
fclose(fp);
*/
// assembled system:
// minimize FObj * beta_tau subject to:
// LeftHand * beta_tau = right_hand
// FO * beta_tau <= inEqZeroes
// beta_tau >= 0
int result = LPSolver(FObj, LeftHand, right_hand, FO, inEqZeroes,
lowerBounds, upperBounds,
beta_tau, objVal);
beta.copySubBlock(0, 0, beta.rows(), 1, beta_tau, 0, 0);
tau.copySubBlock(0, 0, tau.rows(), 1, beta_tau, beta.rows(), 0);
return result;
}
bool
HwcComposer2D::PrepareLayerList(Layer* aLayer,
const nsIntRect& aClip,
const Matrix& aParentTransform)
{
// NB: we fall off this path whenever there are container layers
// that require intermediate surfaces. That means all the
// GetEffective*() coordinates are relative to the framebuffer.
bool fillColor = false;
const nsIntRegion& visibleRegion = aLayer->GetEffectiveVisibleRegion();
if (visibleRegion.IsEmpty()) {
return true;
}
uint8_t opacity = std::min(0xFF, (int)(aLayer->GetEffectiveOpacity() * 256.0));
if (opacity == 0) {
LOGD("%s Layer has zero opacity; skipping", aLayer->Name());
return true;
}
if (!mHal->SupportTransparency() && opacity < 0xFF) {
LOGD("%s Layer has planar semitransparency which is unsupported by hwcomposer", aLayer->Name());
return false;
}
if (aLayer->GetMaskLayer()) {
LOGD("%s Layer has MaskLayer which is unsupported by hwcomposer", aLayer->Name());
return false;
}
nsIntRect clip;
nsIntRect layerClip = aLayer->GetEffectiveClipRect() ?
ParentLayerIntRect::ToUntyped(*aLayer->GetEffectiveClipRect()) : nsIntRect();
nsIntRect* layerClipPtr = aLayer->GetEffectiveClipRect() ? &layerClip : nullptr;
if (!HwcUtils::CalculateClipRect(aParentTransform,
layerClipPtr,
aClip,
&clip))
{
LOGD("%s Clip rect is empty. Skip layer", aLayer->Name());
return true;
}
// HWC supports only the following 2D transformations:
//
// Scaling via the sourceCrop and displayFrame in HwcLayer
// Translation via the sourceCrop and displayFrame in HwcLayer
// Rotation (in square angles only) via the HWC_TRANSFORM_ROT_* flags
// Reflection (horizontal and vertical) via the HWC_TRANSFORM_FLIP_* flags
//
// A 2D transform with PreservesAxisAlignedRectangles() has all the attributes
// above
Matrix layerTransform;
if (!aLayer->GetEffectiveTransform().Is2D(&layerTransform) ||
!layerTransform.PreservesAxisAlignedRectangles()) {
LOGD("Layer EffectiveTransform has a 3D transform or a non-square angle rotation");
return false;
}
Matrix layerBufferTransform;
if (!aLayer->GetEffectiveTransformForBuffer().Is2D(&layerBufferTransform) ||
!layerBufferTransform.PreservesAxisAlignedRectangles()) {
LOGD("Layer EffectiveTransformForBuffer has a 3D transform or a non-square angle rotation");
return false;
}
if (ContainerLayer* container = aLayer->AsContainerLayer()) {
if (container->UseIntermediateSurface()) {
LOGD("Container layer needs intermediate surface");
return false;
}
nsAutoTArray<Layer*, 12> children;
container->SortChildrenBy3DZOrder(children);
for (uint32_t i = 0; i < children.Length(); i++) {
if (!PrepareLayerList(children[i], clip, layerTransform)) {
return false;
}
}
return true;
}
LayerRenderState state = aLayer->GetRenderState();
if (!state.mSurface.get()) {
if (aLayer->AsColorLayer() && mColorFill) {
fillColor = true;
} else {
LOGD("%s Layer doesn't have a gralloc buffer", aLayer->Name());
return false;
}
}
nsIntRect visibleRect = visibleRegion.GetBounds();
nsIntRect bufferRect;
if (fillColor) {
bufferRect = nsIntRect(visibleRect);
} else {
nsIntRect layerRect;
if (state.mHasOwnOffset) {
bufferRect = nsIntRect(state.mOffset.x, state.mOffset.y,
state.mSize.width, state.mSize.height);
layerRect = bufferRect;
} else {
//Since the buffer doesn't have its own offset, assign the whole
//surface size as its buffer bounds
bufferRect = nsIntRect(0, 0, state.mSize.width, state.mSize.height);
layerRect = bufferRect;
if (aLayer->GetType() == Layer::TYPE_IMAGE) {
ImageLayer* imageLayer = static_cast<ImageLayer*>(aLayer);
if(imageLayer->GetScaleMode() != ScaleMode::SCALE_NONE) {
layerRect = nsIntRect(0, 0, imageLayer->GetScaleToSize().width, imageLayer->GetScaleToSize().height);
}
}
}
// In some cases the visible rect assigned to the layer can be larger
// than the layer's surface, e.g., an ImageLayer with a small Image
// in it.
visibleRect.IntersectRect(visibleRect, layerRect);
}
// Buffer rotation is not to be confused with the angled rotation done by a transform matrix
// It's a fancy PaintedLayer feature used for scrolling
if (state.BufferRotated()) {
LOGD("%s Layer has a rotated buffer", aLayer->Name());
return false;
}
const bool needsYFlip = state.OriginBottomLeft() ? true
: false;
hwc_rect_t sourceCrop, displayFrame;
if(!HwcUtils::PrepareLayerRects(visibleRect,
layerTransform,
layerBufferTransform,
clip,
bufferRect,
needsYFlip,
&(sourceCrop),
&(displayFrame)))
{
return true;
}
// OK! We can compose this layer with hwc.
int current = mList ? mList->numHwLayers : 0;
// Do not compose any layer below full-screen Opaque layer
// Note: It can be generalized to non-fullscreen Opaque layers.
bool isOpaque = opacity == 0xFF &&
(state.mFlags & LayerRenderStateFlags::OPAQUE);
// Currently we perform opacity calculation using the *bounds* of the layer.
// We can only make this assumption if we're not dealing with a complex visible region.
bool isSimpleVisibleRegion = visibleRegion.Contains(visibleRect);
if (current && isOpaque && isSimpleVisibleRegion) {
nsIntRect displayRect = nsIntRect(displayFrame.left, displayFrame.top,
displayFrame.right - displayFrame.left, displayFrame.bottom - displayFrame.top);
if (displayRect.Contains(mScreenRect)) {
// In z-order, all previous layers are below
// the current layer. We can ignore them now.
mList->numHwLayers = current = 0;
mHwcLayerMap.Clear();
}
}
if (!mList || current >= mMaxLayerCount) {
if (!ReallocLayerList() || current >= mMaxLayerCount) {
LOGE("PrepareLayerList failed! Could not increase the maximum layer count");
return false;
}
}
HwcLayer& hwcLayer = mList->hwLayers[current];
hwcLayer.displayFrame = displayFrame;
mHal->SetCrop(hwcLayer, sourceCrop);
buffer_handle_t handle = fillColor ? nullptr : state.mSurface->getNativeBuffer()->handle;
hwcLayer.handle = handle;
hwcLayer.flags = 0;
hwcLayer.hints = 0;
hwcLayer.blending = isOpaque ? HWC_BLENDING_NONE : HWC_BLENDING_PREMULT;
#if ANDROID_VERSION >= 17
hwcLayer.compositionType = HWC_FRAMEBUFFER;
hwcLayer.acquireFenceFd = -1;
hwcLayer.releaseFenceFd = -1;
#if ANDROID_VERSION >= 18
hwcLayer.planeAlpha = opacity;
#endif
#else
hwcLayer.compositionType = HwcUtils::HWC_USE_COPYBIT;
#endif
if (!fillColor) {
if (state.FormatRBSwapped()) {
if (!mRBSwapSupport) {
LOGD("No R/B swap support in H/W Composer");
return false;
}
hwcLayer.flags |= HwcUtils::HWC_FORMAT_RB_SWAP;
}
// Translation and scaling have been addressed in PrepareLayerRects().
// Given the above and that we checked for PreservesAxisAlignedRectangles()
// the only possible transformations left to address are
// square angle rotation and horizontal/vertical reflection.
//
// The rotation and reflection permutations total 16 but can be
// reduced to 8 transformations after eliminating redundancies.
//
// All matrices represented here are in the form
//
// | xx xy |
// | yx yy |
//
// And ignore scaling.
//
// Reflection is applied before rotation
gfx::Matrix rotation = layerTransform;
// Compute fuzzy zero like PreservesAxisAlignedRectangles()
if (fabs(rotation._11) < 1e-6) {
if (rotation._21 < 0) {
if (rotation._12 > 0) {
// 90 degree rotation
//
// | 0 -1 |
// | 1 0 |
//
hwcLayer.transform = HWC_TRANSFORM_ROT_90;
LOGD("Layer rotated 90 degrees");
}
else {
// Horizontal reflection then 90 degree rotation
//
// | 0 -1 | | -1 0 | = | 0 -1 |
// | 1 0 | | 0 1 | | -1 0 |
//
// same as vertical reflection then 270 degree rotation
//
// | 0 1 | | 1 0 | = | 0 -1 |
// | -1 0 | | 0 -1 | | -1 0 |
//
hwcLayer.transform = HWC_TRANSFORM_ROT_90 | HWC_TRANSFORM_FLIP_H;
LOGD("Layer vertically reflected then rotated 270 degrees");
}
} else {
if (rotation._12 < 0) {
// 270 degree rotation
//
// | 0 1 |
// | -1 0 |
//
hwcLayer.transform = HWC_TRANSFORM_ROT_270;
LOGD("Layer rotated 270 degrees");
}
else {
// Vertical reflection then 90 degree rotation
//
// | 0 1 | | -1 0 | = | 0 1 |
// | -1 0 | | 0 1 | | 1 0 |
//
// Same as horizontal reflection then 270 degree rotation
//
// | 0 -1 | | 1 0 | = | 0 1 |
// | 1 0 | | 0 -1 | | 1 0 |
//
hwcLayer.transform = HWC_TRANSFORM_ROT_90 | HWC_TRANSFORM_FLIP_V;
LOGD("Layer horizontally reflected then rotated 270 degrees");
}
}
} else if (rotation._11 < 0) {
if (rotation._22 > 0) {
// Horizontal reflection
//
// | -1 0 |
// | 0 1 |
//
hwcLayer.transform = HWC_TRANSFORM_FLIP_H;
LOGD("Layer rotated 180 degrees");
}
else {
// 180 degree rotation
//
// | -1 0 |
// | 0 -1 |
//
// Same as horizontal and vertical reflection
//
// | -1 0 | | 1 0 | = | -1 0 |
// | 0 1 | | 0 -1 | | 0 -1 |
//
hwcLayer.transform = HWC_TRANSFORM_ROT_180;
LOGD("Layer rotated 180 degrees");
}
} else {
if (rotation._22 < 0) {
// Vertical reflection
//
// | 1 0 |
// | 0 -1 |
//
hwcLayer.transform = HWC_TRANSFORM_FLIP_V;
LOGD("Layer rotated 180 degrees");
}
else {
// No rotation or reflection
//
// | 1 0 |
// | 0 1 |
//
hwcLayer.transform = 0;
}
}
const bool needsYFlip = state.OriginBottomLeft() ? true
: false;
if (needsYFlip) {
// Invert vertical reflection flag if it was already set
hwcLayer.transform ^= HWC_TRANSFORM_FLIP_V;
}
hwc_region_t region;
if (visibleRegion.GetNumRects() > 1) {
mVisibleRegions.push_back(HwcUtils::RectVector());
HwcUtils::RectVector* visibleRects = &(mVisibleRegions.back());
bool isVisible = false;
if(!HwcUtils::PrepareVisibleRegion(visibleRegion,
layerTransform,
layerBufferTransform,
clip,
bufferRect,
visibleRects,
isVisible)) {
LOGD("A region of layer is too small to be rendered by HWC");
return false;
}
if (!isVisible) {
// Layer is not visible, no need to render it
return true;
}
region.numRects = visibleRects->size();
region.rects = &((*visibleRects)[0]);
} else {
region.numRects = 1;
region.rects = &(hwcLayer.displayFrame);
}
hwcLayer.visibleRegionScreen = region;
} else {
hwcLayer.flags |= HwcUtils::HWC_COLOR_FILL;
ColorLayer* colorLayer = aLayer->AsColorLayer();
if (colorLayer->GetColor().a < 1.0) {
LOGD("Color layer has semitransparency which is unsupported");
return false;
}
hwcLayer.transform = colorLayer->GetColor().Packed();
}
mHwcLayerMap.AppendElement(static_cast<LayerComposite*>(aLayer->ImplData()));
mList->numHwLayers++;
return true;
}
/*! One possible formulation of the core GFO problem. Checks if some
combination of joint forces exists so that the resultant object
wrench is 0. See inner comments for exact mathematical formulation.
Not for standalone use; is called by the GFO functions in the
Grasp class.
*/
int
graspForceExistence(Matrix &JTD, Matrix &D, Matrix &F, Matrix &G,
Matrix &beta, Matrix &tau, double *objVal)
{
// exact problem formulation
// unknowns: [beta tau] (contact forces and joint forces)
// minimize [beta tau]T*[G 0]T*[G 0]*[beta tau] (magnitude of resultant object wrench)
// subject to:
// [JTD -I] * [beta tau] = 0 (contact forces balance joint forces)
// [0 sum] * [beta tau] = 1 (we are applying some joint forces)
// [F 0] [beta tau] <= 0 (all forces inside friction cones)
// [beta tau] >= 0 (all forces must be positive)
// overall equality constraint:
// | JTD -I | | beta | |0|
// | 0 sum| | tau | = |1|
int numJoints = tau.rows();
Matrix beta_tau(beta.rows() + tau.rows(), 1);
//right-hand side of equality constraint
Matrix right_hand( JTD.rows() + 1, 1);
right_hand.setAllElements(0.0);
//actually, we use 1.0e8 here as units are in N * 1.0e-6 * mm
//so we want a total joint torque of 100 N mm
right_hand.elem( right_hand.rows()-1, 0) = 1.0e8;
//left-hand side of equality constraint
Matrix LeftHand( JTD.rows() + 1, D.cols() + numJoints);
LeftHand.setAllElements(0.0);
LeftHand.copySubMatrix(0, 0, JTD);
LeftHand.copySubMatrix(0, D.cols(), Matrix::NEGEYE(numJoints, numJoints) );
for (int i=0; i<numJoints; i++) {
LeftHand.elem( JTD.rows(), D.cols() + i) = 1.0;
}
//matrix F padded with zeroes for tau
//left hand side of the inequality matrix
Matrix FO(F.rows(), F.cols() + numJoints);
FO.setAllElements(0.0);
FO.copySubMatrix(0, 0, F);
//right-hand side of inequality matrix
Matrix inEqZeroes(FO.rows(), 1);
inEqZeroes.setAllElements(0.0);
//objective matrix: G padded with zeroes
Matrix GO(Matrix::ZEROES<Matrix>(G.rows(), G.cols() + numJoints));
GO.copySubMatrix(0, 0, G);
//bounds: all variables greater than 0
// CHANGE: only beta >= 0, tau is not
Matrix lowerBounds(Matrix::MIN_VECTOR(beta_tau.rows()));
lowerBounds.copySubMatrix( 0, 0, Matrix::ZEROES<Matrix>(beta.rows(), 1) );
Matrix upperBounds(Matrix::MAX_VECTOR(beta_tau.rows()));
/*
FILE *fp = fopen("gfo.txt","w");
fprintf(fp,"left hand:\n");
LeftHand.print(fp);
fprintf(fp,"right hand:\n");
right_hand.print(fp);
fprintf(fp,"friction inequality:\n");
FO.print(fp);
fprintf(fp,"Objective:\n");
GO.print(fp);
fclose(fp);
*/
// assembled system:
// minimize beta_tauT*QOT*QO*beta_tau subject to:
// LeftHand * beta_tau = right_hand
// FO * beta_tau <= inEqZeroes
// beta_tau >= 0
// CHANGE: only beta >= 0, tau is not
int result = factorizedQPSolver(GO, LeftHand, right_hand, FO, inEqZeroes,
lowerBounds, upperBounds,
beta_tau, objVal);
beta.copySubBlock(0, 0, beta.rows(), 1, beta_tau, 0, 0);
tau.copySubBlock(0, 0, tau.rows(), 1, beta_tau, beta.rows(), 0);
return result;
}
returnValue MultiObjectiveAlgorithm::solveSingleObjective( const int &number_ ){
int run1 ;
returnValue returnvalue;
ASSERT( ocp != 0 );
ASSERT( number_ < m );
if( N == 0 ) get( PARETO_FRONT_DISCRETIZATION, N );
int N_pow_m_minus_1 = 1;
int i;
for(i=0; i<m-1; i++)
N_pow_m_minus_1 *= N;
Expression **arg = 0;
arg = new Expression*[m];
for( run1 = 0; run1 < m; run1++ )
ocp->getObjective( run1, &arg[run1] );
Grid tmp_grid;
ocp->getGrid( tmp_grid );
Objective tmp(tmp_grid);
tmp.addMayerTerm(*arg[number_]);
ocp->setObjective( tmp );
setStatus( BS_NOT_INITIALIZED );
acadoPrintf("\n\n Optimization of individual objective %d out of %d \n\n",number_ +1, m );
totalCPUtime = -acadoGetTime();
returnvalue = OptimizationAlgorithm::solve();
totalCPUtime += acadoGetTime();
int index;
Matrix Weights = getWeights();
for( run1 = 0; run1 < (int) Weights.getNumCols(); run1++ ){
if( fabs( Weights( number_, run1 ) - 1.0 ) < 1000.0*EPS )
index = run1;
}
if( xResults == 0 ) xResults = new VariablesGrid[Weights.getNumCols()];
if( xaResults == 0 ) xaResults = new VariablesGrid[Weights.getNumCols()];
if( pResults == 0 ) pResults = new VariablesGrid[Weights.getNumCols()];
if( uResults == 0 ) uResults = new VariablesGrid[Weights.getNumCols()];
if( wResults == 0 ) wResults = new VariablesGrid[Weights.getNumCols()];
getDifferentialStates( xResults[index] );
getAlgebraicStates ( xaResults[index] );
getParameters ( pResults[index] );
getControls ( uResults[index] );
getDisturbances ( wResults[index] );
if( returnvalue != SUCCESSFUL_RETURN )
return ACADOERROR(returnvalue);
if( nlpSolver != 0 )
totalNumberOfSQPiterations = nlpSolver->getNumberOfSteps();
int hotstart;
get( PARETO_FRONT_HOTSTART, hotstart );
int *indices = new int[m];
for( run1 = 0; run1 < m; run1++ )
indices[run1] = number_;
VariablesGrid xd_tmp, xa_tmp, p_tmp, u_tmp, w_tmp;
if( hotstart == BT_TRUE ){
getDifferentialStates( *userInit.x );
getAlgebraicStates ( *userInit.xa );
getParameters ( *userInit.p );
getControls ( *userInit.u );
getDisturbances ( *userInit.w );
xd_tmp = *userInit.x;
xa_tmp = *userInit.xa;
p_tmp = *userInit.p;
u_tmp = *userInit.u;
w_tmp = *userInit.w;
}
else{
getDifferentialStates( xd_tmp );
getAlgebraicStates ( xa_tmp );
getParameters ( p_tmp );
getControls ( u_tmp );
getDisturbances ( w_tmp );
}
VariablesGrid *_xd = 0;
VariablesGrid *_xa = 0;
VariablesGrid *_p = 0;
VariablesGrid *_u = 0;
VariablesGrid *_w = 0;
if( xd_tmp.isEmpty() == BT_FALSE ) _xd = new VariablesGrid(xd_tmp);
if( xa_tmp.isEmpty() == BT_FALSE ) _xa = new VariablesGrid(xa_tmp);
if( p_tmp.isEmpty() == BT_FALSE ) _p = new VariablesGrid(p_tmp );
if( u_tmp.isEmpty() == BT_FALSE ) _u = new VariablesGrid(u_tmp );
if( w_tmp.isEmpty() == BT_FALSE ) _w = new VariablesGrid(w_tmp );
if( vertices.getDim() == 0 ){
vertices.init(m,m);
vertices.setAll(-INFTY);
}
Objective **obj = new Objective*[m];
for( run1 = 0; run1 < m; run1++ ){
obj[run1] = new Objective( tmp_grid );
obj[run1]->addMayerTerm(*arg[run1]);
OCPiterate xx( _xd, _xa, _p, _u, _w );
obj[run1]->evaluate( xx );
obj[run1]->getObjectiveValue( vertices(number_,run1) );
}
if( _xd != 0 ) delete _xd;
if( _xa != 0 ) delete _xa;
if( _p != 0 ) delete _p ;
if( _u != 0 ) delete _u ;
if( _w != 0 ) delete _w ;
delete[] indices;
for( run1 = 0; run1 < m; run1++ )
delete arg[run1];
delete[] arg;
for( run1 = 0; run1 < m; run1++ )
delete obj[run1];
delete[] obj;
return SUCCESSFUL_RETURN;
}
Matrix operator+(double c1, const Matrix &c2)
{
return c2.add(c1);
}
returnValue MultiObjectiveAlgorithm::solve( ){
int run1,run2;
returnValue returnvalue;
ASSERT( ocp != 0 );
ASSERT( m >= 2 );
if( N == 0 ) get( PARETO_FRONT_DISCRETIZATION, N );
int paretoGeneration;
get( PARETO_FRONT_GENERATION, paretoGeneration );
int hotstart;
get( PARETO_FRONT_HOTSTART, hotstart );
Expression **arg = 0;
arg = new Expression*[m];
for( run1 = 0; run1 < m; run1++ )
ocp->getObjective( run1, &arg[run1] );
Constraint tmp_con;
double *idx = new double[m];
WeightGeneration generator;
Matrix Weights;
Vector formers;
Vector lb(m);
Vector ub(m);
lb.setZero();
ub.setAll(1.0);
generator.getWeights( m, N, lb, ub, Weights, formers );
result.init( Weights.getNumCols(), m );
count = 0;
if( xResults == 0 ) xResults = new VariablesGrid[Weights.getNumCols()];
if( xaResults == 0 ) xaResults = new VariablesGrid[Weights.getNumCols()];
if( pResults == 0 ) pResults = new VariablesGrid[Weights.getNumCols()];
if( uResults == 0 ) uResults = new VariablesGrid[Weights.getNumCols()];
if( wResults == 0 ) wResults = new VariablesGrid[Weights.getNumCols()];
totalNumberOfSQPiterations = 0;
totalCPUtime = -acadoGetTime();
run1 = 0;
while( run1 < (int) Weights.getNumCols() ){
// PRINT THE ITERATION NUMBER:
// ---------------------------
acadoPrintf("\n\n Multi-objective point: %d out of %d \n\n",run1+1, (int) Weights.getNumCols() );
ocp->getConstraint( tmp_con );
for( run2 = 0; run2 < (int) Weights.getNumRows(); run2++ )
idx[run2] = Weights( run2, run1 );
// THIS PART OF THE CODE WILL NOT RUN YET FOR GENERAL WEIGHTS
int vertex = -1;
for( run2 = 0; run2 < m; run2++ ){
if( fabs( idx[run2]-1.0 ) < 100.0*EPS )
vertex = run2;
}
// ----------------------------------------------------------
if( vertex == -1 || paretoGeneration == PFG_WEIGHTED_SUM ){
formulateOCP( idx, ocp, arg );
setStatus( BS_NOT_INITIALIZED );
returnvalue = OptimizationAlgorithm::solve();
if( nlpSolver != 0 )
totalNumberOfSQPiterations += nlpSolver->getNumberOfSteps();
ocp->setConstraint( tmp_con );
set( PRINT_COPYRIGHT, BT_FALSE );
if( returnvalue != SUCCESSFUL_RETURN ){
ACADOERROR(returnvalue);
}
else{
getDifferentialStates( xResults[run1] );
getAlgebraicStates ( xaResults[run1] );
getParameters ( pResults[run1] );
getControls ( uResults[run1] );
getDisturbances ( wResults[run1] );
if( hotstart == BT_TRUE ){
getDifferentialStates( *userInit.x );
getAlgebraicStates ( *userInit.xa );
getParameters ( *userInit.p );
getControls ( *userInit.u );
getDisturbances ( *userInit.w );
evaluateObjectives( *userInit.x, *userInit.xa, *userInit.p, *userInit.u, *userInit.w, arg );
}
else{
VariablesGrid xd_tmp, xa_tmp, p_tmp, u_tmp, w_tmp;
getDifferentialStates( xd_tmp );
getAlgebraicStates ( xa_tmp );
getParameters ( p_tmp );
getControls ( u_tmp );
getDisturbances ( w_tmp );
evaluateObjectives( xd_tmp, xa_tmp, p_tmp, u_tmp, w_tmp, arg );
}
}
}
else{
acadoPrintf(" Result from single objective optimization is adopted. \n\n" );
for( run2 = 0; run2 < m; run2++ ){
result(count,run2) = vertices(vertex,run2);
}
count++;
}
run1++;
}
totalCPUtime += acadoGetTime();
for( run1 = 0; run1 < m; run1++ )
delete arg[run1];
delete[] arg;
delete[] idx;
return SUCCESSFUL_RETURN;
}
Matrix operator-(const Matrix &c1, double c2)
{
return c1.subtract(c2);
}
returnValue MultiObjectiveAlgorithm::formulateOCP( double *idx, OCP *ocp_, Expression **arg ){
int run1, run2;
int paretoGeneration;
double factor;
get( PARETO_FRONT_GENERATION, paretoGeneration );
if( paretoGeneration == PFG_UNKNOWN ) paretoGeneration = PFG_WEIGHTED_SUM;
Grid tmp_grid;
ocp_->getGrid( tmp_grid );
Objective tmp(tmp_grid);
// WEIGTHED SUM:
// -----------------------------------------------------------------------------
if( paretoGeneration == PFG_WEIGHTED_SUM ){
Expression sum(1);
for( run1 = 0; run1 < m; run1++ ){ // loop over the number of objectives
factor = idx[run1]; // determines the weight factor
sum = sum + arg[run1][0]*factor;
}
tmp.addMayerTerm(sum); // add the new objective as a Meyer Term
ocp_->setObjective(tmp); // replace (overwrite) the objective in the ocp
return SUCCESSFUL_RETURN;
}
// NORMALIZED NORMAL CONSTRAINT:
// -----------------------------------------------------------------------------
if( paretoGeneration == PFG_NORMALIZED_NORMAL_CONSTRAINT ){
// Normalization based on Messac et al 2004
//tmp.addMayerTerm( *arg[m-1] ); // Select last (i.e., m-th) objective function
//ocp_->setObjective( tmp ); // replace (overwrite) the objective in the ocp
Matrix P = getNormalizedPayOffMatrix();
Matrix NK = getUtopiaPlaneVectors ();
Vector W(m);
for( run1 = 0; run1 < m; run1++ )
W(run1) = idx[run1];
Vector PW = P*W;
Vector U = getUtopiaVector();
Vector L = getNormalizationVector();
Expression *Fnorm;
Fnorm = new Expression[m];
for( run2 = 0; run2 < m; run2++ ){
Fnorm[run2] = ( *arg[run2] - U(run2) ) / L(run2);
}
tmp.addMayerTerm( Fnorm[m-1] );
ocp_->setObjective( tmp );
for( run1 = 0; run1 < m-1; run1++ ){
Expression sum(1);
for( run2 = 0; run2 < m; run2++ ){
sum = sum + NK(run2,run1)*( Fnorm[run2] - PW(run2) );
}
ocp_->subjectTo( AT_END, sum <= 0.0 );
}
delete[] Fnorm;
return SUCCESSFUL_RETURN;
}
// ENHANCED NORMALIZED NORMAL CONSTRAINT:
// -----------------------------------------------------------------------------
if( paretoGeneration == PFG_ENHANCED_NORMALIZED_NORMAL_CONSTRAINT ){
// Normalization based on Sanchis et al 2008
Matrix P = getPayOffMatrix();
Matrix PHI_N(m,m);
int run3, run4;
for( run3 = 0; run3 < m; run3++ ){
for( run4 = 0; run4 < m; run4++ ){
PHI_N(run3,run4) = 1.0;
}
}
for( run3 = 0; run3 < m; run3++ )
PHI_N(run3,run3) = 0.0;
Matrix T;
T = PHI_N*P.getInverse();
Matrix NK(m,m-1);
NK.setZero();
for( run3 = 0; run3 < m-1; run3++ )
NK(run3,run3) = 1.0;
for( run3 = 0; run3 < m-1; run3++ )
NK(m-1,run3) = -1.0;
Vector W(m);
for( run1 = 0; run1 < m; run1++ )
W(run1) = idx[run1];
Vector PW = PHI_N*W;
Vector U = getUtopiaVector();
Expression *Fnorm;
Fnorm = new Expression[m];
for( run2 = 0; run2 < m; run2++ ){
Expression tmp3(1);
for( run3 = 0; run3 < m; run3++ ){
tmp3 = tmp3 + T(run2,run3)*( *arg[run3]- U(run3) );
}
Fnorm[run2] = tmp3;
}
tmp.addMayerTerm( Fnorm[m-1] );
ocp_->setObjective( tmp );
for( run1 = 0; run1 < m-1; run1++ ){
Expression sum(1);
for( run2 = 0; run2 < m; run2++ ){
sum = sum + NK(run2,run1)*( Fnorm[run2] - PW(run2) );
}
ocp_->subjectTo( AT_END, sum <= 0.0 );
}
delete[] Fnorm;
return SUCCESSFUL_RETURN;
}
// NORMAL BOUNDARY INTERSECTION:
// -----------------------------------------------------------------------------
if( paretoGeneration == PFG_NORMAL_BOUNDARY_INTERSECTION ){
Vector W(m);
for( run1 = 0; run1 < m; run1++ )
W(run1) = idx[run1];
Matrix P = getPayOffMatrix();
Vector U = getUtopiaVector();
Vector V = P*W + U;
W.setAll( 1.0 );
Vector X = P*W;
Expression lambda;
lambda = ( *arg[m-1] - V(m-1) )/ X(m-1);
tmp.addMayerTerm( *arg[m-1] );
ocp_->setObjective( tmp );
for( run1 = 0; run1 < m-1; run1++ )
ocp->subjectTo( AT_END, *arg[run1] - lambda*X(run1) == V(run1) );
return SUCCESSFUL_RETURN;
}
return SUCCESSFUL_RETURN;
}
Matrix operator*(const Matrix &c1, double c2)
{
return c1.multiply(c2);
}
inline void
TwoSidedTrsmUVar2( UnitOrNonUnit diag, Matrix<F>& A, const Matrix<F>& U )
{
#ifndef RELEASE
PushCallStack("internal::TwoSidedTrsmUVar2");
if( A.Height() != A.Width() )
throw std::logic_error("A must be square");
if( U.Height() != U.Width() )
throw std::logic_error("Triangular matrices must be square");
if( A.Height() != U.Height() )
throw std::logic_error("A and U must be the same size");
#endif
// Matrix views
Matrix<F>
ATL, ATR, A00, A01, A02,
ABL, ABR, A10, A11, A12,
A20, A21, A22;
Matrix<F>
UTL, UTR, U00, U01, U02,
UBL, UBR, U10, U11, U12,
U20, U21, U22;
// Temporary products
Matrix<F> Y01;
PartitionDownDiagonal
( A, ATL, ATR,
ABL, ABR, 0 );
LockedPartitionDownDiagonal
( U, UTL, UTR,
UBL, UBR, 0 );
while( ATL.Height() < A.Height() )
{
RepartitionDownDiagonal
( ATL, /**/ ATR, A00, /**/ A01, A02,
/*************/ /******************/
/**/ A10, /**/ A11, A12,
ABL, /**/ ABR, A20, /**/ A21, A22 );
LockedRepartitionDownDiagonal
( UTL, /**/ UTR, U00, /**/ U01, U02,
/*************/ /******************/
/**/ U10, /**/ U11, U12,
UBL, /**/ UBR, U20, /**/ U21, U22 );
//--------------------------------------------------------------------//
// Y01 := A00 U01
Zeros( A01.Height(), A01.Width(), Y01 );
Hemm( LEFT, UPPER, F(1), A00, U01, F(0), Y01 );
// A01 := A01 - 1/2 Y01
Axpy( F(-1)/F(2), Y01, A01 );
// A11 := A11 - (U01' A01 + A01' U01)
Her2k( UPPER, ADJOINT, F(-1), U01, A01, F(1), A11 );
// A11 := inv(U11)' A11 inv(U11)
TwoSidedTrsmUUnb( diag, A11, U11 );
// A12 := A12 - A02' U01
Gemm( ADJOINT, NORMAL, F(-1), A02, U01, F(1), A12 );
// A12 := inv(U11)' A12
Trsm( LEFT, UPPER, ADJOINT, diag, F(1), U11, A12 );
// A01 := A01 - 1/2 Y01
Axpy( F(-1)/F(2), Y01, A01 );
// A01 := A01 inv(U11)
Trsm( RIGHT, UPPER, NORMAL, diag, F(1), U11, A01 );
//--------------------------------------------------------------------//
SlidePartitionDownDiagonal
( ATL, /**/ ATR, A00, A01, /**/ A02,
/**/ A10, A11, /**/ A12,
/*************/ /******************/
ABL, /**/ ABR, A20, A21, /**/ A22 );
SlideLockedPartitionDownDiagonal
( UTL, /**/ UTR, U00, U01, /**/ U02,
/**/ U10, U11, /**/ U12,
/*************/ /******************/
UBL, /**/ UBR, U20, U21, /**/ U22 );
}
#ifndef RELEASE
PopCallStack();
#endif
}
static void solve(FluidState &fluidState,
const typename MaterialLaw::Params &matParams,
typename FluidSystem::template ParameterCache<typename FluidState::Scalar>& paramCache,
const Dune::FieldVector<typename FluidState::Scalar, numComponents>& globalMolarities,
Scalar tolerance = -1)
{
typedef typename FluidState::Scalar InputEval;
/////////////////////////
// Check if all fluid phases are incompressible
/////////////////////////
bool allIncompressible = true;
for (unsigned phaseIdx = 0; phaseIdx < numPhases; ++phaseIdx) {
if (FluidSystem::isCompressible(phaseIdx)) {
allIncompressible = false;
break;
}
}
if (allIncompressible) {
// yes, all fluid phases are incompressible. in this case the flash solver
// can only determine the saturations, not the pressures. (but this
// determination is much simpler than a full flash calculation.)
paramCache.updateAll(fluidState);
solveAllIncompressible_(fluidState, paramCache, globalMolarities);
return;
}
typedef Dune::FieldMatrix<InputEval, numEq, numEq> Matrix;
typedef Dune::FieldVector<InputEval, numEq> Vector;
typedef Opm::LocalAd::Evaluation<InputEval, numEq> FlashEval;
typedef Dune::FieldVector<FlashEval, numEq> FlashDefectVector;
typedef Opm::ImmiscibleFluidState<FlashEval, FluidSystem> FlashFluidState;
Dune::FMatrixPrecision<InputEval>::set_singular_limit(1e-35);
if (tolerance <= 0)
tolerance = std::min<Scalar>(1e-5,
1e8*std::numeric_limits<Scalar>::epsilon());
typename FluidSystem::template ParameterCache<FlashEval> flashParamCache;
flashParamCache.assignPersistentData(paramCache);
/////////////////////////
// Newton method
/////////////////////////
// Jacobian matrix
Matrix J;
// solution, i.e. phase composition
Vector deltaX;
// right hand side
Vector b;
Valgrind::SetUndefined(J);
Valgrind::SetUndefined(deltaX);
Valgrind::SetUndefined(b);
FlashFluidState flashFluidState;
assignFlashFluidState_<MaterialLaw>(fluidState, flashFluidState, matParams, flashParamCache);
// copy the global molarities to a vector of evaluations. Remember that the
// global molarities are constants. (but we need to copy them to a vector of
// FlashEvals anyway in order to avoid getting into hell's kitchen.)
Dune::FieldVector<FlashEval, numComponents> flashGlobalMolarities;
for (unsigned compIdx = 0; compIdx < numComponents; ++ compIdx)
flashGlobalMolarities[compIdx] = globalMolarities[compIdx];
FlashDefectVector defect;
const unsigned nMax = 50; // <- maximum number of newton iterations
for (unsigned nIdx = 0; nIdx < nMax; ++nIdx) {
// calculate Jacobian matrix and right hand side
evalDefect_(defect, flashFluidState, flashGlobalMolarities);
Valgrind::CheckDefined(defect);
// create field matrices and vectors out of the evaluation vector to solve
// the linear system of equations.
for (unsigned eqIdx = 0; eqIdx < numEq; ++ eqIdx) {
for (unsigned pvIdx = 0; pvIdx < numEq; ++ pvIdx)
J[eqIdx][pvIdx] = defect[eqIdx].derivatives[pvIdx];
b[eqIdx] = defect[eqIdx].value;
}
Valgrind::CheckDefined(J);
Valgrind::CheckDefined(b);
// Solve J*x = b
deltaX = 0;
try { J.solve(deltaX, b); }
catch (Dune::FMatrixError e) {
throw Opm::NumericalProblem(e.what());
}
Valgrind::CheckDefined(deltaX);
// update the fluid quantities.
Scalar relError = update_<MaterialLaw>(flashFluidState, flashParamCache, matParams, deltaX);
if (relError < tolerance) {
assignOutputFluidState_(flashFluidState, fluidState);
return;
}
}
OPM_THROW(Opm::NumericalProblem,
"ImmiscibleFlash solver failed: "
"{c_alpha^kappa} = {" << globalMolarities << "}, "
<< "T = " << fluidState.temperature(/*phaseIdx=*/0));
}
const Matrix&
J2BeamFiber2d::getTangent (void)
{
double twoG = E/(1.0+nu);
double G = 0.5*twoG;
double sig[2];
sig[0] = E*(Tepsilon(0)-epsPn[0]);
sig[1] = G*(Tepsilon(1)-epsPn[1]);
static const double one3 = 1.0/3;
static const double two3 = 2.0*one3;
static const double root23 = sqrt(two3);
double two3Hkin = two3*Hkin;
double xsi[2];
//xsi[0] = sig[0] - two3*Hkin*1.5*epsPn[0];
//xsi[1] = sig[1] - two3*Hkin*0.5*epsPn[1];
xsi[0] = sig[0] - Hkin*epsPn[0];
xsi[1] = sig[1] - one3*Hkin*epsPn[1];
double q = sqrt(two3*xsi[0]*xsi[0] + 2.0*xsi[1]*xsi[1]);
double F = q - root23*(sigmaY + Hiso*alphan);
if (F < -100*DBL_EPSILON) {
D(0,0) = E;
D(1,1) = G;
D(0,1) = D(1,0) = 0.0;
epsPn1[0] = epsPn[0];
epsPn1[1] = epsPn[1];
}
else {
// Solve for dg
double dg = 0.0;
static Vector R(3);
R(0) = 0.0; R(1) = 0.0; R(2) = F;
static Vector x(3);
x(0) = xsi[0]; x(1) = xsi[1]; x(2) = dg;
static Matrix J(3,3);
static Vector dx(3);
int iter = 0; int maxIter = 25;
while (iter < maxIter && R.Norm() > sigmaY*1.0e-14) {
iter++;
J(0,0) = 1.0 + dg*two3*(E+Hkin); J(0,1) = 0.0;
J(1,0) = 0.0; J(1,1) = 1.0 + dg*(twoG+two3Hkin);
J(0,2) = two3*(E+Hkin)*x(0);
J(1,2) = (twoG+two3Hkin)*x(1);
//J(2,0) = x(0)*two3/q; J(2,1) = x(1)*2.0/q;
J(2,0) = (1.0-two3*Hiso*dg)*x(0)*two3/q;
J(2,1) = (1.0-two3*Hiso*dg)*x(1)*2.0/q;
//J(2,2) = -root23*Hiso;
J(2,2) = -two3*Hiso*q;
J.Solve(R, dx);
x.addVector(1.0, dx, -1.0);
dg = x(2);
dg_n1 = dg;
q = sqrt(two3*x(0)*x(0) + 2.0*x(1)*x(1));
R(0) = x(0) - xsi[0] + dg*two3*(E+Hkin)*x(0);
R(1) = x(1) - xsi[1] + dg*(twoG+two3Hkin)*x(1);
R(2) = q - root23*(sigmaY + Hiso*(alphan+dg*root23*q));
}
if (iter == maxIter) {
//opserr << "J2BeamFiber2d::getTangent -- maxIter reached " << R.Norm() << endln;
}
alphan1 = alphan + dg*root23*q;
epsPn1[0] = epsPn[0] + dg*two3*x(0);
epsPn1[1] = epsPn[1] + dg*2.0*x(1);
//J(2,0) = (1.0-two3*Hiso*dg)*x(0)*two3/q; J(2,1) = (1.0-two3*Hiso*dg)*x(1)*2.0/q;
//J(2,2) = -two3*Hiso*q;
//static Matrix invJ(3,3);
//J.Invert(invJ);
J(0,0) = 1.0 + dg*two3*E/(1.0+dg*two3Hkin); J(0,1) = 0.0;
J(1,0) = 0.0; J(1,1) = 1.0 + dg*twoG/(1.0+dg*two3Hkin);
J(0,2) = (two3*E-dg*two3*E/(1.0+dg*two3Hkin)*two3Hkin)*x(0);
J(1,2) = (twoG -dg* twoG/(1.0+dg*two3Hkin)*two3Hkin)*x(1);
//J(2,0) = x(0)/q*two3/(1.0+dg*two3Hkin);
//J(2,1) = x(1)/q* 2.0/(1.0+dg*two3Hkin);
J(2,0) = (1.0-two3*Hiso*dg)*x(0)/q*two3/(1.0+dg*two3Hkin);
J(2,1) = (1.0-two3*Hiso*dg)*x(1)/q* 2.0/(1.0+dg*two3Hkin);
//J(2,2) = -(x(0)/q*two3/(1.0+dg*two3Hkin)*two3Hkin*x(0))
// -(x(1)/q* 2.0/(1.0+dg*two3Hkin)*two3Hkin*x(1));
//J(2,2) = -q*two3Hkin/(1.0+dg*two3Hkin) - root23*Hiso;
J(2,2) = -q*two3Hkin/(1.0+dg*two3Hkin) - two3*Hiso*q;
static Matrix invJ(3,3);
J.Invert(invJ);
D(0,0) = invJ(0,0)*E;
D(1,0) = invJ(1,0)*E;
D(0,1) = invJ(0,1)*G;
D(1,1) = invJ(1,1)*G;
}
return D;
}
