Exemplo n.º 1
0
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;
}
Exemplo n.º 2
0
Matrix operator*(double c1, const Matrix &c2)
{
    return c2.multiply(c1);
}
Exemplo n.º 3
0
Matrix operator*(const vec &c2, const Matrix &c1)
{
    return c1.multiply(c2);
}
Exemplo n.º 4
0
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;
}
Exemplo n.º 5
0
Matrix operator+(const Matrix &c1, double c2)
{
    return c1.add(c2);
}
Exemplo n.º 6
0
/*! 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;
}
Exemplo n.º 7
0
/*! 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;
}
Exemplo n.º 8
0
 void uniformMatrix4fv(GLint uniform, Matrix mat)
 {
     DirectX::XMFLOAT4X4 fMat;
     XMStoreFloat4x4(&fMat, mat.getXMMatrix());
     glUniformMatrix4fv(uniform, 1, GL_FALSE, &fMat.m[0][0]);
 }
Exemplo n.º 9
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;
    }
}
Exemplo n.º 10
0
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;
}
Exemplo n.º 11
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;
}
Exemplo n.º 12
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 );
        }
    }
}
Exemplo n.º 13
0
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;
}
Exemplo n.º 14
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;
}
Exemplo n.º 15
0
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;
}
Exemplo n.º 16
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;
 }
Exemplo n.º 17
0
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;
}
Exemplo n.º 18
0
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;
}
Exemplo n.º 19
0
/*! 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;
}
Exemplo n.º 20
0
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;
}
Exemplo n.º 21
0
/*! 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;
}
Exemplo n.º 22
0
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;
}
Exemplo n.º 23
0
Matrix operator+(double c1, const Matrix &c2)
{
    return c2.add(c1);
}
Exemplo n.º 24
0
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;
}
Exemplo n.º 25
0
Matrix operator-(const Matrix &c1, double c2)
{
    return c1.subtract(c2);
}
Exemplo n.º 26
0
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;
}
Exemplo n.º 27
0
Matrix operator*(const Matrix &c1, double c2)
{
    return c1.multiply(c2);
}
Exemplo n.º 28
0
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
}
Exemplo n.º 29
0
    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));
    }
Exemplo n.º 30
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;
}