Example #1
0
void Stwart(vector<long>&IA,vector<long>&MA,
            vector<long>&JROW,vector<long>&JCOL)
{
  static long M = JROW.size();
  vector<long> IR(M),IC(M),R(M),C(M),IP(M),JP(M),IW(M);
  static long KERNS, MEND, LG, IER, L = MA[M];
  // printf("M = %ld\n",M);
  // forVector(MA,i) printf("MA[%ld] = %ld\n",i,MA[i]);
  // forVector(IA,i) printf("IA[%ld] = %ld\n",i,IA[i]);
  // printf("L=%ld\n",L);
  // forVector(R,i) printf("R[%ld] = %ld\n",i,R[i]);
  // forVector(C,i) printf("C[%ld] = %ld\n",i,C[i]);
  // forVector(IR,i) printf("IR[%ld] = %ld\n",i,IR[i]);
  // forVector(IC,i) printf("IC[%ld] = %ld\n",i,IC[i]);
  // forVector(JROW,i) printf("JROW[%ld] = %ld\n",i,JROW[i]);
  // forVector(JCOL,i) printf("JCOL[%ld] = %ld\n",i,JCOL[i]);
  // forVector(IP,i) printf("IP[%ld] = %ld\n",i,IP[i]);
  // forVector(JP,i) printf("JP[%ld] = %ld\n",i,JP[i]);
  // forVector(IW,i) printf("IW[%ld] = %ld\n",i,IW[i]);
  printf("KERNS = %ld, MEND = %ld, LG = %ld, IER = %ld\n",
	 KERNS, MEND, LG, IER);
  

  stwart_(&IA[0],&L,&MA[0],&M,&R[0],&C[0],&IR[0],&IC[0],
          &JROW[0],&JCOL[0],&IP[0],&JP[0],&KERNS,&MEND,&IW[0],&LG,&IER);
}
Example #2
0
size_t
SpatialEdge::newEdge(size_t emindex, size_t index, int k)
{
  Edge *en, *em;
  size_t swap;

  em = &edges_[emindex];

  switch (k) {
  case 0:
    em->start_ = IV(1);
    em->end_   = IV(2);
    break;
  case 1:
    em->start_ = IV(0);
    em->end_   = IV(2);
    break;
  case 2:
    em->start_ = IV(0);
    em->end_   = IV(1);
    break;
  }

  // sort the vertices by increasing index

  if(em->start_ > em->end_) {
    swap = em->start_;
    em->start_ = em->end_;
    em->end_ = swap;
  }

  // check all previous edges for a match, return pointer if 
  // already present, log the midpoint with the new face as well
   
  if( (en=edgeMatch(em)) != NULL){
    IW(k) = en->mid_;
    return emindex;
  }

// this is a new edge, immediately process the midpoint, 
// and save it with the nodes and the edge as well

  insertLookup(em);
  IW(k)      = getMidPoint(em);
  em->mid_   = IW(k);
  return ++emindex;
}
Example #3
0
static void
dec_1000a_disable_intr(int irq)
{
	int imrval = IRQ2IMR(irq);
	int i = imrval >= 16;

	IW(i, IR(i) & ~(1 << (imrval & 0xf)));
}
Example #4
0
void pSubChain::calc_acc_body()
{
//	if(!children[1]) return;
#ifdef USE_MPI
	if(sim->rank != rank) return;
	if(children[0] && sim->rank != children[0]->rank)
	{
		children[0]->recv_acc();
	}
	if(children[1] && children[0] != children[1] && sim->rank != children[1]->rank)
	{
		children[1]->recv_acc();
	}
#endif
//	update_log << "--- " << last_joint->name << ": calc_acc_body" << endl;
	int i, j;
	// compute f_temp
	static fVec da;
	static fMat PK;
	PK.resize(6, n_dof);
	da.resize(n_const);
	for(i=0; i<6; i++)
	{
		for(j=0; j<n_dof; j++)
			PK(i, j) = P(i, joint_index[j]);
	}
	if(last_joint->n_dof > 0)
	{
		switch(last_joint->j_type)
		{
		case JROTATE:
		case JSLIDE:
			tau(0) = last_joint->tau;
			break;
		case JSPHERE:
			tau(0) = last_joint->tau_n(0);
			tau(1) = last_joint->tau_n(1);
			tau(2) = last_joint->tau_n(2);
			break;
		case JFREE:
			tau(0) = last_joint->tau_f(0);
			tau(1) = last_joint->tau_f(1);
			tau(2) = last_joint->tau_f(2);
			tau(3) = last_joint->tau_n(0);
			tau(4) = last_joint->tau_n(1);
			tau(5) = last_joint->tau_n(2);
			break;
		default:
			break;
		}
		da6.mul(PK, tau);
	}
	else
		da6.zero();
//	cerr << "da6(0) = " << tran(da6) << endl;
	// + child_side - parent_side ?
	da6 += children[0]->acc_temp[last_index[0]];
//	cerr << "da6(1) = " << tran(da6) << endl;
	if(children[1])
		da6 -= children[1]->acc_temp[last_index[1]];
//	cerr << "da6(2) = " << tran(da6) << endl;
	// motion controlled joints
	if(!last_joint->t_given)
	{
		switch(last_joint->j_type)
		{
		case JROTATE:
		case JSLIDE:
			da6(axis) -= last_joint->qdd;
//			update_log << last_joint->name << ": qdd = " << last_joint->qdd << endl;
			break;
		case JSPHERE:
			da6(3) -= last_joint->rel_ang_acc(0);
			da6(4) -= last_joint->rel_ang_acc(1);
			da6(5) -= last_joint->rel_ang_acc(2);
			break;
		case JFREE:
			da6(0) -= last_joint->rel_lin_acc(0);
			da6(1) -= last_joint->rel_lin_acc(1);
			da6(2) -= last_joint->rel_lin_acc(2);
			da6(3) -= last_joint->rel_ang_acc(0);
			da6(4) -= last_joint->rel_ang_acc(1);
			da6(5) -= last_joint->rel_ang_acc(2);
			break;
		}
	}
	static fVec f(6);
//	cerr << "Gamma = " << Gamma << endl;
//	cerr << "Gamma_inv = " << Gamma_inv << endl;
#if 0
	// actually we could save some computation by
	// selecting const rows first
	for(i=0; i<n_const; i++)
		da(i) = -da6(const_index[i]);
	f_temp.mul(Gamma_inv, da);
//	f_temp.lineq_posv(Gamma, da);
	// compute acc at all outer joints
	for(i=0; i<n_dof; i++)
		f(joint_index[i]) = tau(i);
	for(i=0; i<n_const; i++)
		f(const_index[i]) = f_temp(i);
//	cerr << "da = " << tran(da) << endl;
//	cerr << "f_temp = " << tran(f_temp) << endl;
//	cerr << "Gamma*f_temp - da = " << tran(Gamma*f_temp-da) << endl;
#else
#if 0
	f.mul(W, da6);
	for(i=0; i<n_dof; i++)
	{
		f(joint_index[i]) += tau(i);
	}
#else
	static fVec db(6), Wdb(6), IWRtau(6);
	static fMat IWR;
	IWR.resize(6, n_dof);
	db.set(children[0]->acc_temp[last_index[0]]);
	if(children[1])
		db -= children[1]->acc_temp[last_index[1]];
	Wdb.mul(W, db);
	for(i=0; i<6; i++)
	{
		for(j=0; j<n_dof; j++)
		{
			IWR(i, j) = IW(joint_index[j], i);
		}
	}
	IWRtau.mul(IWR, tau);
//	cerr << "W = " << tran(W) << endl;
//	update_log << "db = " << tran(db) << endl;
//	cerr << "Wdb = " << tran(Wdb) << endl;
//	cerr << "IWRtau = " << tran(IWRtau) << endl;
	f.add(Wdb, IWRtau);
//	update_log << "f = " << tran(f) << endl;
	
#ifdef PSIM_TEST
	////// -> test
	for(i=0; i<n_const; i++)
	{
		da(i) = -da6(const_index[i]);
		f_temp(i) = f(const_index[i]);
	}
//	cerr << "Gamma*f_temp - da = " << tran(Gamma*f_temp-da) << endl;
	total_gamma_error += (Gamma*f_temp-da) * (Gamma*f_temp-da);
	////// <-
#endif
#endif
#endif
	for(i=0; i<n_outer_joints; i++)
	{
		int org = outer_joints_origin[i];
		int index = outer_joints_index[i];
		int ilast = last_index[org];
		acc_temp[i].mul(children[org]->Lambda[index][ilast], f);
		if(org == 1)
		{
			acc_temp[i] *= -1.0;
		}
		acc_temp[i] += children[org]->acc_temp[index];
//		update_log << "acc_temp[" << i << "] = " << tran(acc_temp[i]) << endl;
	}
#ifdef USE_MPI
	if(parent && sim->rank != parent->rank)
	{
		send_acc(parent->rank);
	}
#endif
}
Example #5
0
void pSubChain::calc_inertia_body()
{
	// for space
//	if(!children[1]) return;
#ifdef USE_MPI
	if(sim->rank != rank) return;
	if(children[0] && sim->rank != children[0]->rank)
	{
//		cerr << sim->rank << ": joint (" << last_joint->name << "): recv_inertia from " << children[0]->rank << endl;
		children[0]->recv_inertia();
	}
	if(children[1] && children[0] != children[1] && sim->rank != children[1]->rank)
	{
//		cerr << sim->rank << ": joint (" << last_joint->name << "): recv_inertia from " << children[1]->rank << endl;
		children[1]->recv_inertia();
	}
#endif
	int i, j;
//	cerr << "---- " << last_joint->name << ": calc_inertia_body" << endl;
	// P
	if(children[1])
	{
		P.add(children[0]->Lambda[last_index[0]][last_index[0]], children[1]->Lambda[last_index[1]][last_index[1]]);
	}
	else
	{
		P.set(children[0]->Lambda[last_index[0]][last_index[0]]);
	}
//	cerr << "Lambda[0] = " << children[0]->Lambda[last_index[0]][last_index[0]] << endl;
//	cerr << "Lambda[1] = " << children[1]->Lambda[last_index[1]][last_index[1]] << endl;
#ifdef PSIM_TEST
	{
		fMat U1(6,6), V1T(6,6), U2(6,6), V2T(6,6);
		fVec sigma1(6), sigma2(6);
		children[0]->Lambda[last_index[0]][last_index[0]].svd(U1, sigma1, V1T);
		children[1]->Lambda[last_index[1]][last_index[1]].svd(U2, sigma2, V2T);
		double s1 = sigma1.length(), s2 = sigma2.length();
		if(s1 > 1e-8 && s2 > 1e-8)
		{
			double ratio = (s1 < s2) ? s2/s1 : s1/s2;
			if(max_sigma_ratio < 0.0 || ratio > max_sigma_ratio)
			{
				max_sigma_ratio = ratio;
				max_sigma_ratio_joint = last_joint;
			}
			sigma_ratios(last_joint->i_dof) =  ratio;
//			cerr << last_joint->name << ": " << s1 << ", " << s2 << " -> " << ratio << endl;
		}
	}
#endif
	if(children[0] == children[1])
	{
		P -= children[0]->Lambda[last_index[0]][last_index[1]];
		P -= children[0]->Lambda[last_index[1]][last_index[0]];
	}
	// P should be symmetric
//	P += tran(P);
//	P *= 0.5;
	P.symmetric();
#ifndef USE_DCA
	// Gamma
	if(n_const > 0)
	{
		for(i=0; i<n_const; i++)
		{
			for(j=0; j<n_const; j++)
				Gamma(i, j) = P(const_index[i], const_index[j]);
		}
		if(children[0] != children[1])
		{
			// Gamma is symmetric, positive-definite
			if(Gamma_inv.inv_posv(Gamma))
				Gamma_inv.inv_svd(Gamma);
//			Gamma_inv.inv_porfs(Gamma);
#ifdef PSIM_TEST
			fMat U(n_const, n_const), VT(n_const, n_const);
			fVec sigma(n_const);
			Gamma.svd(U, sigma, VT);
			double cn = sigma(0) / sigma(n_const-1);
			if(max_condition_number < 0.0 || cn > max_condition_number)
			{
				max_condition_number = cn;
				max_condition_number_joint = last_joint;
			}
			condition_numbers(last_joint->i_dof) = cn;
//			cerr << "condition_number = " << cn << endl;
//			fMat Gamma_inv2(n_const, n_const);
//			Gamma_inv2.inv_svd(Gamma, 2000);
//			cerr << last_joint->name << ": " << cn << endl;
//			fMat I(n_const, n_const);
//			I.identity();
//			cerr << last_joint->name << ": " << I - Gamma*Gamma_inv << endl;
//			cerr << last_joint->name << ": " << Gamma_inv - Gamma_inv2 << endl;
#endif
		}
		else
		{
			// Gamma may be singular
			Gamma_inv.inv_svd(Gamma);
//			cerr << Gamma_inv * Gamma << endl;
		}
	}
	// W, IW
	W.zero();
	for(i=0; i<n_const; i++)
	{
		for(int j=0; j<n_const; j++)
		{
			W(const_index[i], const_index[j]) = -Gamma_inv(i, j);
		}
	}
#else // #ifndef USE_DCA
	static fMat SV, VSV;
	SV.resize(n_dof, 6);
	VSV.resize(n_dof, 6);
	Vhat.inv_posv(P);
	if(n_dof > 0)
	{
		for(i=0; i<n_dof; i++)
		{
			for(int j=0; j<n_dof; j++)
			{
				SVS(i,j) = Vhat(joint_index[i], joint_index[j]);
			}
			for(int j=0; j<6; j++)
			{
				SV(i,j) = Vhat(joint_index[i], j);
			}
		}
//		cerr << "SVS = " << SVS << endl;
		VSV.lineq(SVS, SV);
//		cerr << "VSV = " << VSV << endl;
		W.mul(tran(SV), VSV);
//		W *= -1.0;
	}
	else
	{
		W.zero();
	}
	W -= Vhat;
#endif
//	cerr << "P = " << P << endl;
//	cerr << "W = " << W << endl;
	IW.mul(P, W);
	for(i=0; i<6; i++)
	{
		IW(i, i) += 1.0;
	}
//	cerr << "IW = " << IW << endl;
	// Lambda
	if(n_const == 0)
	{
		for(i=0; i<n_outer_joints; i++)
		{
			int org_i = outer_joints_origin[i];
			int index_i = outer_joints_index[i];
			for(j=i; j<n_outer_joints; j++)
			{
				int org_j = outer_joints_origin[j];
				int index_j = outer_joints_index[j];
				if(children[org_i] == children[org_j])
				{
					Lambda[i][j].set(children[org_i]->Lambda[index_i][index_j]);
				}
				if(i != j)
				{
					Lambda[j][i].tran(Lambda[i][j]);
				}
			}
		}
	}
	else
	{
		for(i=0; i<n_outer_joints; i++)
		{
			int org_i = outer_joints_origin[i];
			int index_i = outer_joints_index[i];
			fMat& Lambda_i = children[org_i]->Lambda[index_i][last_index[org_i]];
#ifndef USE_DCA
			int m, n;
			static fMat LKi, KLj, GKLj;
			LKi.resize(6, n_const);
			KLj.resize(n_const, 6);
			GKLj.resize(n_const, 6);
			for(m=0; m<6; m++)
			{
				for(n=0; n<n_const; n++)
					LKi(m, n) = Lambda_i(m, const_index[n]);
			}
			for(j=i; j<n_outer_joints; j++)
			{
				int org_j = outer_joints_origin[j];
				int index_j = outer_joints_index[j];
				fMat& Lambda_j = children[org_j]->Lambda[last_index[org_j]][index_j];
				for(m=0; m<n_const; m++)
				{
					for(n=0; n<6; n++)
						KLj(m, n) = Lambda_j(const_index[m], n);
				}
				GKLj.mul(Gamma_inv, KLj);
//				GKLj.lineq_posv(Gamma, KLj);
				Lambda[i][j].mul(LKi, GKLj);
				if(children[org_i] == children[org_j])
				{
					Lambda[i][j] -= children[org_i]->Lambda[index_i][index_j];
					Lambda[i][j] *= -1.0;
				}
				if(i != j)
				{
					Lambda[j][i].tran(Lambda[i][j]);
				}
				else
				{
					Lambda[i][j].symmetric();
				}
			}
#else  // #ifndef USE_DCA
			for(j=i; j<n_outer_joints; j++)
			{
				int org_j = outer_joints_origin[j];
				int index_j = outer_joints_index[j];
				fMat& Lambda_j = children[org_j]->Lambda[last_index[org_j]][index_j];
				static fMat WL(6,6);
				WL.mul(W, Lambda_j);
				WL *= -1.0;
				Lambda[i][j].mul(Lambda_i, WL);
				if(children[org_i] == children[org_j])
				{
					Lambda[i][j] -= children[org_i]->Lambda[index_i][index_j];
					Lambda[i][j] *= -1.0;
				}
				if(i != j)
				{
					Lambda[j][i].tran(Lambda[i][j]);
				}
				else
				{
					Lambda[i][j].symmetric();
				}
			}
#endif
		}
	}
#ifdef USE_MPI
	if(parent && sim->rank != parent->rank)
	{
//		cerr << sim->rank << ": joint (" << last_joint->name << "): send_inertia to " << parent->rank << endl;
		send_inertia(parent->rank);
	}
#endif
}
Example #6
0
/*
 * Initialize mystery ICU
 */
static void
pci_1000a_imi(void)
{
	IW(0, IR(0) & 1);
	IW(1, IR(0) & 3);
}
void
MAST::NPSOLOptimizationInterface::optimize() {
    
#if MAST_ENABLE_NPSOL == 1
    // make sure that functions have been provided
    libmesh_assert(_funobj);
    libmesh_assert(_funcon);
    
    int
    N      =  _feval->n_vars(),
    NCLIN  =  0,
    NCNLN  =  _feval->n_eq()+_feval->n_ineq(),
    NCTOTL =  N+NCLIN+NCNLN,
    LDA    =  std::max(NCLIN, 1),
    LDJ    =  std::max(NCNLN, 1),
    LDR    =  N,
    INFORM =  0,           // on exit: Reports result of call to NPSOL
                           // < 0 either funobj or funcon has set this to -ve
                           // 0 => converged to point x
                           // 1 => x satisfies optimality conditions, but sequence of iterates has not converged
                           // 2 => Linear constraints and bounds cannot be satisfied. No feasible solution
                           // 3 => Nonlinear constraints and bounds cannot be satisfied. No feasible solution
                           // 4 => Major iter limit was reached
                           // 6 => x does not satisfy first-order optimality to required accuracy
                           // 7 => function derivatives seem to be incorrect
                           // 9 => input parameter invalid
    ITER   = 0,            // iter count
    LENIW  = 3*N + NCLIN + 2*NCNLN,
    LENW   = 2*N*N + N*NCLIN + 2*N*NCNLN + 20*N + 11*NCLIN + 21*NCNLN;
    
    Real
    F      =  0.;          // on exit: final objective

    std::vector<int>
    IW      (LENIW,  0),
    ISTATE  (NCTOTL, 0);    // status of constraints l <= r(x) <= u,
                            // -2 => lower bound is violated by more than delta
                            // -1 => upper bound is violated by more than delta
                            // 0  => both bounds are satisfied by more than delta
                            // 1  => lower bound is active (to within delta)
                            // 2  => upper bound is active (to within delta)
                            // 3  => boundars are equal and equality constraint is satisfied
    
    std::vector<Real>
    A       (LDA,    0.),   // this is used for liear constraints, not currently handled
    BL      (NCTOTL, 0.),
    BU      (NCTOTL, 0.),
    C       (NCNLN,  0.),   // on exit: nonlinear constraints
    CJAC    (LDJ* N, 0.),   //
                            // on exit: CJAC(i,j) is the partial derivative of ith nonlinear constraint
    CLAMBDA (NCTOTL, 0.),   // on entry: need not be initialized for cold start
                            // on exit: QP multiplier from the QP subproblem, >=0 if istate(j)=1, <0 if istate(j)=2
    G       (N,      0.),   // on exit: objective gradient
    R       (LDR*N,  0.),   // on entry: need not be initialized if called with Cold Statrt
                            // on exit: information about Hessian, if Hessian=Yes, R is upper Cholesky factor of approx H
    X       (N,      0.),   // on entry: initial point
                            // on exit: final estimate of solution
    W       (LENW,   0.),   // workspace
    xmin    (N,      0.),
    xmax    (N,      0.);
    
    
    // now setup the lower and upper limits for the variables and constraints
    _feval->init_dvar(X, xmin, xmax);
    for (unsigned int i=0; i<N; i++) {
        BL[i] = xmin[i];
        BU[i] = xmax[i];
    }
    
    // all constraints are assumed to be g_i(x) <= 0, so that the upper
    // bound is 0 and lower bound is -infinity
    for (unsigned int i=0; i<NCNLN; i++) {
        BL[i+N] = -1.e20;
        BU[i+N] =     0.;
    }
    
    std::string nm;
//    nm = "List";
//    npoptn_(nm.c_str(), (int)nm.length());
//    nm = "Verify level 3";
//    npoptn_(nm.c_str(), (int)nm.length());
    
    npsol_(&N,
           &NCLIN,
           &NCNLN,
           &LDA,
           &LDJ,
           &LDR,
           &A[0],
           &BL[0],
           &BU[0],
           _funcon,
           _funobj,
           &INFORM,
           &ITER,
           &ISTATE[0],
           &C[0],
           &CJAC[0],
           &CLAMBDA[0],
           &F,
           &G[0],
           &R[0],
           &X[0],
           &IW[0],
           &LENIW,
           &W[0],
           &LENW);
    
#endif // MAST_ENABLE_NPSOL 1
}