C++ Matrix类代码示例

const Vector&
J2BeamFiber2d::getStress (void)
{
  double G = 0.5*E/(1.0+nu);

  sigma(0) = E*(Tepsilon(0)-epsPn[0]);
  sigma(1) = G*(Tepsilon(1)-epsPn[1]);

  static const double one3 = 1.0/3;
  static const double two3 = 2.0*one3;
  static const double root23 = sqrt(two3);

  double xsi[2];
  //xsi[0] = sigma(0) - two3*Hkin*1.5*epsPn[0];
  //xsi[1] = sigma(1) - two3*Hkin*0.5*epsPn[1];
  xsi[0] = sigma(0) -      Hkin*epsPn[0];
  xsi[1] = sigma(1) - one3*Hkin*epsPn[1];

  double q = sqrt(two3*xsi[0]*xsi[0] + 2.0*xsi[1]*xsi[1]);
  double F = q - root23*(sigmaY + Hiso*alphan);

  if (F < -100*DBL_EPSILON) {
    epsPn1[0] = epsPn[0];
    epsPn1[1] = epsPn[1];
  }
  else {

    // Solve for dg
    double dg = 0.0;

    static Vector R(3);
    R(0) = 0.0; R(1) = 0.0; R(2) = F;
    static Vector x(3);
    x(0) = xsi[0]; x(1) = xsi[1]; x(2) = dg;

    static Matrix J(3,3);
    static Vector dx(3);

    int iter = 0; int maxIter = 25;
    while (iter < maxIter && R.Norm() > sigmaY*1.0e-14) {
        iter++;

        J(0,0) = 1.0 + dg*two3*(E+Hkin); J(0,1) = 0.0;
        J(1,0) = 0.0; J(1,1) = 1.0 + dg*(2.0*G+two3*Hkin);

        J(0,2) = two3*(E+Hkin)*x(0);
        J(1,2) = (2.0*G+two3*Hkin)*x(1);

        //J(2,0) = x(0)*two3/q; J(2,1) = x(1)*2.0/q;
        J(2,0) = (1.0-two3*Hiso*dg)*x(0)*two3/q;
        J(2,1) = (1.0-two3*Hiso*dg)*x(1)*2.0/q;

        //J(2,2) = -root23*Hiso;
	J(2,2) = -two3*Hiso*q;

        J.Solve(R, dx);
        x = x-dx;

        dg = x(2);
        dg_n1 = dg;

        q = sqrt(two3*x(0)*x(0) + 2.0*x(1)*x(1));

        R(0) = x(0) - xsi[0] + dg*two3*(E+Hkin)*x(0);
        R(1) = x(1) - xsi[1] + dg*(2.0*G+two3*Hkin)*x(1);
        R(2) = q - root23*(sigmaY + Hiso*(alphan+dg*root23*q));
    }

    if (iter == maxIter) {
      //opserr << "J2BeamFiber2d::getStress -- maxIter reached " << R.Norm() << endln;
    }

    alphan1 = alphan + dg*root23*q;

    epsPn1[0] = epsPn[0] + dg*two3*x(0);
    epsPn1[1] = epsPn[1] + dg*2.0*x(1);

    //sigma(0) = x(0) + two3*Hkin*1.5*epsPn1[0];
    //sigma(1) = x(1) + two3*Hkin*0.5*epsPn1[1];
    sigma(0) = x(0) +      Hkin*epsPn1[0];
    sigma(1) = x(1) + one3*Hkin*epsPn1[1];
  }

  return sigma;
}

Matrix operator*(double c1, const Matrix &c2)
{
    return c2.multiply(c1);
}

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];
//.........这里部分代码省略.........

/*! One possible formulation of the core GFO problem. Finds the contacts
	forces that result in 0 object wrench and are as far as possible 
	from the edges of the friction cones. Assumes that at least one set
	of contact forces that satisfy this criterion exist; see 
	contactForceExistence for that problem. See inner comments for exact 
	mathematical formulation. Not for standalone use; is called by the GFO 
	functions in the Grasp class.
*/
int
contactForceOptimization(Matrix &F, Matrix &N, Matrix &Q, Matrix &beta, double *objVal, Matrix * objectWrench = NULL)
{
	// exact problem formulation
	// unknowns: beta					(contact forces)
	// minimize sum * F * beta			(crude measure of friction resistance abilities)
	// subject to:
	// Q * beta = 0						(0 resultant object wrench)
	// sum_normal * beta = 1			(we are applying some contact forces)
	// F * beta <= 0					(each individual forces inside friction cones)
	// beta >= 0	  				    (all forces must be positive)
	// overall equality constraint:
	// | Q | |beta|   |0|
	// | N |        = |1|

	//right hand of equality
        Matrix right_hand = Matrix::ZEROES<Matrix>(Q.rows()+1, 1);
	if(objectWrench)
	  {
	
	    assert(objectWrench->rows() == Q.rows());
	  
	    for(int i = 0; i < Q.rows(); ++i)
	      right_hand.elem(i,0) = objectWrench->elem(i,0);
	  }
	  
	//a total of 10N of normal force
	right_hand.elem(Q.rows(),0) = 1.0e7;

	//left hand of equality
	Matrix LeftHand(Q.rows() + 1, Q.cols());
	LeftHand.copySubMatrix(0, 0, Q);
	LeftHand.copySubMatrix(Q.rows(), 0, N);

	//bounds: all variables greater than 0
	Matrix lowerBounds(Matrix::ZEROES<Matrix>(beta.rows(),1));
	Matrix upperBounds(Matrix::MAX_VECTOR(beta.rows()));

	//objective: sum of F
	Matrix FSum(1,F.rows());
	FSum.setAllElements(1.0);
	Matrix FObj(1,F.cols());
	matrixMultiply(FSum, F, FObj);
	/*
	FILE *fp = fopen("gfo.txt","w");
	fprintf(fp,"Left Hand:\n");
	LeftHand.print(fp);
	fprintf(fp,"right hand:\n");
	right_hand.print(fp);
	fprintf(fp,"friction inequality:\n");
	F.print(fp);
	fprintf(fp,"Objective:\n");
	Q.print(fp);
	fclose(fp);
	*/
	int result = LPSolver(FObj, LeftHand, right_hand, F, Matrix::ZEROES<Matrix>(F.rows(), 1), 
						  lowerBounds, upperBounds, 
						  beta, objVal);
	return result;
}

/*! One possible version of the GFO problem.

	Given a matrix of joint torques applied to the robot joints, this will check
	if there exists a set of legal contact forces that balance them. If so, it
	will compute the set of contact forces that adds up to the wrench of least
	magnitude on the object.

	For now, this output of this function is to set the computed contact forces
	on each contact as dynamic forces, and also to accumulate the resulting
	wrench on the object in its external wrench accumulator.

	Return codes: 0 is success, >0 means finger slip, no legal contact forces 
	exist; <0 means error in computation 
*/
int 
Grasp::computeQuasistaticForces(const Matrix &robotTau)
{
	//WARNING: for now, this ignores contacts on the palm. That might change in the future

	//for now, if the hand is touching anything other then the object, abort
	for (int c=0; c<hand->getNumChains(); c++) {
		if ( hand->getChain(c)->getNumContacts(NULL) !=
			hand->getChain(c)->getNumContacts(object) ) {
				DBGA("Hand contacts not on object");
				return 1;
			}
	}

	std::list<Contact*> contacts;
	std::list<Joint*> joints;

	bool freeChainForces = false;
	for(int c=0; c<hand->getNumChains(); c++) {
		//for now, we look at all contacts
		std::list<Contact*> chainContacts = hand->getChain(c)->getContacts(object);
		contacts.insert(contacts.end(), chainContacts.begin(), chainContacts.end());
		if (!chainContacts.empty()) {
			std::list<Joint*> chainJoints = hand->getChain(c)->getJoints();
			joints.insert(joints.end(), chainJoints.begin(), chainJoints.end());
		} else {
			//the chain has no contacts
			//check if any joint forces are not 0
			Matrix chainTau = hand->getChain(c)->jointTorquesVector(robotTau);
			//torque units should be N * 1.0e6 * mm
			if (chainTau.absMax() > 1.0e3) {
				DBGA("Joint torque " << chainTau.absMax() << " on chain " << c 
									 << " with no contacts");
				freeChainForces = true;
			}
		}
	}
	//if there are no contacts, nothing to compute!
	if (contacts.empty()) return 0;

	//assemble the joint forces matrix
	Matrix tau((int)joints.size(), 1);
	int jc; std::list<Joint*>::iterator jit;
	for (jc=0, jit = joints.begin(); jit!=joints.end(); jc++,jit++) {
		tau.elem(jc,0) = robotTau.elem( (*jit)->getNum(), 0 );
	}
	//if all the joint forces we care about are zero, do an early exit 
	//as zero contact forces are guaranteed to balance the chain
	//we should probably be able to use a much larger threshold here, if
	//units are what I think they are
	if (tau.absMax() < 1.0e-3) {
		return 0;
	}

	//if there are forces on chains with no contacts, we have no hope to balance them
	if (freeChainForces) {
		return 1;
	}

	Matrix J(contactJacobian(joints, contacts));
	Matrix D(Contact::frictionForceBlockMatrix(contacts));
	Matrix F(Contact::frictionConstraintsBlockMatrix(contacts));
	Matrix R(Contact::localToWorldWrenchBlockMatrix(contacts));

	//grasp map G = S*R*D
	Matrix G(graspMapMatrix(R, D));

	//left-hand equality matrix JTD = JTran * D
	Matrix JTran(J.transposed());
	Matrix JTD(JTran.rows(), D.cols());
	matrixMultiply(JTran, D, JTD);

	//matrix of zeroes for right-hand of friction inequality
	Matrix zeroes(Matrix::ZEROES<Matrix>(F.rows(), 1));

	//matrix of unknowns
	Matrix c_beta(D.cols(), 1);

	//bounds: all variables greater than 0
	Matrix lowerBounds(Matrix::ZEROES<Matrix>(D.cols(),1));
	Matrix upperBounds(Matrix::MAX_VECTOR(D.cols()));
	/*
	//test for sparse matrices
	SparseMatrix JTDSparse(JTD.rows(), JTD.cols());
	JTDSparse.copyMatrix(JTD);
	DBGA("Dense matrix: \n" << JTD << "size: " << JTD.rows()*JTD.cols());
//.........这里部分代码省略.........

 void uniformMatrix4fv(GLint uniform, Matrix mat)
 {
     DirectX::XMFLOAT4X4 fMat;
     XMStoreFloat4x4(&fMat, mat.getXMMatrix());
     glUniformMatrix4fv(uniform, 1, GL_FALSE, &fMat.m[0][0]);
 }

void SVDLinearSolver<TMatrix,TVector>::solve(Matrix& M, Vector& x, Vector& b)
{
#ifdef SOFA_DUMP_VISITOR_INFO
    simulation::Visitor::printComment("SVD");
#endif
#ifdef DISPLAY_TIME
    CTime * timer;
    double time1 = (double) timer->getTime();
#endif
    const bool printLog = this->f_printLog.getValue();
    const bool verbose  = f_verbose.getValue();

    /// Convert the matrix and the right-hand vector to Eigen objects
    Eigen::MatrixXd m(M.rowSize(),M.colSize());
    Eigen::VectorXd rhs(M.rowSize());
    for(unsigned i=0; i<M.rowSize(); i++ )
    {
        for( unsigned j=0; j<M.colSize(); j++ )
            m(i,j) = M[i][j];
        rhs(i) = b[i];
    }
    if(verbose)
    {
        serr << "SVDLinearSolver<TMatrix,TVector>::solve, Here is the matrix m:" << sendl << m << sendl;
    }

    /// Compute the SVD decomposition and the condition number
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(m, Eigen::ComputeThinU | Eigen::ComputeThinV);
    f_conditionNumber.setValue( (Real)(svd.singularValues()(0) / svd.singularValues()(M.rowSize()-1)) );
    if(printLog)
    {
        serr << "SVDLinearSolver<TMatrix,TVector>::solve, the singular values are:" << sendl << svd.singularValues() << sendl;
    }
    if(verbose)
    {
        serr << "Its left singular vectors are the columns of the thin U matrix:" << sendl << svd.matrixU() << sendl;
        serr << "Its right singular vectors are the columns of the thin V matrix:" << sendl << svd.matrixV() << sendl;
    }

    /// Solve the equation system and copy the solution to the SOFA vector
//    Eigen::VectorXd solution = svd.solve(rhs);
//    for(unsigned i=0; i<M.rowSize(); i++ ){
//        x[i] = solution(i);
//    }
    Eigen::VectorXd Ut_b = svd.matrixU().transpose() *  rhs;
    Eigen::VectorXd S_Ut_b(M.colSize());
    for( unsigned i=0; i<M.colSize(); i++ )   /// product with the diagonal matrix, using the threshold for near-null values
    {
        if( svd.singularValues()[i] > f_minSingularValue.getValue() )
            S_Ut_b[i] = Ut_b[i]/svd.singularValues()[i];
        else
            S_Ut_b[i] = (Real)0.0 ;
    }
    Eigen::VectorXd solution = svd.matrixV() * S_Ut_b;
    for(unsigned i=0; i<M.rowSize(); i++ )
    {
        x[i] = (Real) solution(i);
    }

    if( printLog )
    {
#ifdef DISPLAY_TIME
        time1 = (double)(((double) timer->getTime() - time1) * timeStamp / (nb_iter-1));
        std::cerr<<"SVDLinearSolver::solve, SVD = "<<time1<<std::endl;
#endif
        serr << "SVDLinearSolver<TMatrix,TVector>::solve, rhs vector = " << sendl << rhs.transpose() << sendl;
        serr << "SVDLinearSolver<TMatrix,TVector>::solve, solution = " << sendl << x << sendl;
        serr << "SVDLinearSolver<TMatrix,TVector>::solve, verification, mx - b = " << sendl << (m * solution - rhs ).transpose() << sendl;
    }
}

int main()
{
    Cube c = Cube();
    char* s = (char*)malloc(256*sizeof(char));

    char* fun = (char*)malloc(256*sizeof(char));
    float* params = (float*)malloc(10*sizeof(float));
    int len=0;

    char* word;

    c.reset();

    do {
        cout<<"$ ";
        cin.getline(s, 256, '\n');

        if (!strcmp(s,"quit"))break;

        if (strcmp(s,"r"))
        {
            word = strtok (s," ");

            if (word == NULL) continue;

            len = 0;

            if (strcmp(word,"l"))
            {
                strcpy(fun, word);
            }


            while (len<9 && word != NULL)
            {
                word = strtok (NULL, " ");
                if (word!=NULL)
                {
                    params[len++] = atof(word);
                }
            }
        }



        if (!strcmp(fun, "reset")) {
            c.reset();
        } else if (!strcmp(fun, "translateX") && len>=1) {
            c.translateX(params[0]);
        } else if (!strcmp(fun, "translateY") && len>=1) {
            c.translateY(params[0]);
        } else if (!strcmp(fun, "translateZ") && len>=1) {
            c.translateZ(params[0]);
        } else if (!strcmp(fun, "rotateH") && len>=1) {
            c.rotateH(params[0]);
        } else if (!strcmp(fun, "rotateP") && len>=1) {
            c.rotateP(params[0]);
        } else if (!strcmp(fun, "rotateR") && len>=1) {
            c.rotateR(params[0]);
        }
        else if (!strcmp(fun, "scale") && len>=1) {
            c.scale(params[0]);
        } else if (!strcmp(fun, "rotateHPR") && len>=3) {
            c.rotateHPR(params[0], params[1], params[2]);
        } else if (!strcmp(fun, "translateXYZ") && len>=3) {
            c.translateXYZ(params[0], params[1], params[2]);
        } else if (!strcmp(fun, "transformXYZHPRS") && len>=7) {
            c.transformXYZHPRS(params[0], params[1], params[2],
                               params[3], params[4], params[5], params[6]);
        } else if (!strcmp(fun, "toQuat")) {
            c.mat->toQuat().print();
            continue;
        } else if (!strcmp(fun, "slerp_mat") && len>=10) {
            Matrix m;
            m.data[0][0] = params[0];
            m.data[1][0] = params[1];
            m.data[2][0] = params[2];
            m.data[0][1] = params[3];
            m.data[1][1] = params[4];
            m.data[2][1] = params[5];
            m.data[0][2] = params[6];
            m.data[1][2] = params[7];
            m.data[2][2] = params[8];
            m.slerp(*(c.mat), params[9]).print();
            continue;
        } else if (!strcmp(fun, "slerp_quat") && len>=5) {
            Quaternion q;
            q.x = params[0];
            q.y = params[1];
            q.z = params[2];
            q.w = params[3];

            q.slerp(*(c.mat), params[4]).print();
            continue;
        } else if (strcmp(fun, "display")) {
            cout << "Commands:\n" <<
                 "\treset\n" <<
                 "\tdisplay\n" <<
                 "\ttranslateX [x]\n" <<
                 "\ttranslateY [y]\n" <<
//.........这里部分代码省略.........

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();
//.........这里部分代码省略.........

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.
//.........这里部分代码省略.........

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;
}

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++)
//.........这里部分代码省略.........

 static Matrix<float, 3, 24> wireframe(const float scale) {
     Matrix<float, 3, 24> m;
     m.col(0) << -1, -1, -1;
     m.col(1) << -1, -1, +1;
     m.col(2) << -1, -1, -1;
     m.col(3) << -1, +1, -1;
     m.col(4) << -1, -1, -1;
     m.col(5) << +1, -1, -1;
     m.col(6) << -1, -1, +1;
     m.col(7) << -1, +1, +1;
     m.col(8) << -1, -1, +1;
     m.col(9) << +1, -1, +1;
     m.col(10) << -1, +1, -1;
     m.col(11) << -1, +1, +1;
     m.col(12) << -1, +1, -1;
     m.col(13) << +1, +1, -1;
     m.col(14) << -1, +1, +1;
     m.col(15) << +1, +1, +1;
     m.col(16) << +1, -1, -1;
     m.col(17) << +1, -1, +1;
     m.col(18) << +1, -1, -1;
     m.col(19) << +1, +1, -1;
     m.col(20) << +1, -1, +1;
     m.col(21) << +1, +1, +1;
     m.col(22) << +1, +1, -1;
     m.col(23) << +1, +1, +1;
     m *= scale;
     return m;
 }

LRESULT WINAPI WndProc(HWND hWnd, UINT uMsg, WPARAM wParam, LPARAM lParam){
	LRESULT  lRet = 1;
	int wmId, wmEvent;
	PAINTSTRUCT ps;
	HDC hdc;

	switch (uMsg){
		case WM_CREATE:
			break;
		case WM_PAINT:
			ValidateRect(g_wnd, NULL);
			break;
		case WM_DESTROY:
			PostQuitMessage(0);
			break;
		case WM_CHAR:
			break;
		case WM_LBUTTONDOWN:
		{
			POINT ptMouse;
			GetCursorPos(&ptMouse);
			ScreenToClient(g_wnd, &ptMouse);
			g_ptLastPoint = ptMouse;
		}
		break;
		case WM_RBUTTONDOWN:
		{
			GetCursorPos(&g_ptLastPoint);
			ScreenToClient(g_wnd, &g_ptLastPoint);
		break;
		}
		case WM_MOUSEMOVE:
		{
			switch (wParam)
			{
			case MK_LBUTTON:
			{

			}
			break;
			case MK_RBUTTON:
			{
			POINT pt;
			pt.x = LOWORD(lParam);
			pt.y = HIWORD(lParam);
			g_camera->SetRotAngleDelta((pt.y - g_ptLastPoint.y) / 150.0f, (pt.x - g_ptLastPoint.x) / 150.0f, 0.0f);
			g_ptLastPoint = pt;
			}
			default:
			break;
			}
			break;
			}
			case WM_KEYDOWN:
			{
			Vector *vcDirc = new Vector();
			Vector *vcUp = new Vector();
			Vector *vcRight = new Vector();
			g_camera->GetDirection(vcDirc, vcUp, vcRight);
			switch (wParam)
			{
			case VK_A:
			{
			g_camera->SetMoveDirection(*vcRight);
			g_camera->SetMoveDelta(-20.0f);

			g_camera->Update();
			Matrix matView;
			g_camera->GetViewMatrix(&matView);
			break;
			}
			case VK_D:
			{
			Vector vcPosCamera;
			g_camera->SetMoveDirection(*vcRight);
			g_camera->SetMoveDelta(20.0f);
			g_camera->Update();
			Matrix matView;
			g_camera->GetViewMatrix(&matView);
			break;
			}
			case VK_W:
			{
			g_camera->SetMoveDirection(*vcDirc);
			g_camera->SetMoveDelta(20.0f);

			g_camera->Update();
			Matrix matView;
			g_camera->GetViewMatrix(&matView);
			break;
			}
			case VK_S:
			{
			g_camera->SetMoveDirection(*vcDirc);
			g_camera->SetMoveDelta(-20.0f);

			g_camera->Update();
			Matrix matView;
			g_camera->GetViewMatrix(&matView);
			break;
//.........这里部分代码省略.........

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;
}