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); }
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; }
static void dec_1000a_disable_intr(int irq) { int imrval = IRQ2IMR(irq); int i = imrval >= 16; IW(i, IR(i) & ~(1 << (imrval & 0xf))); }
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 }
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 }
/* * 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 }