void PFEMElement2DBubble::getdK(Matrix& dk) const { double J2 = J/2.*thickness; // double lambda = -2*mu/3.0; dk.resize(6,6); dk.Zero(); double dNdx[3], dNdy[3]; for(int a=0; a<3; a++) { dNdx[a] = dJ(2*a)/J; dNdy[a] = dJ(2*a+1)/J; } // other matrices for(int a=0; a<3; a++) { for(int b=0; b<3; b++) { dk(2*a, 2*b) += J2*(2*dNdx[a]*dNdx[b] + dNdy[a]*dNdy[b]); // K1 dk(2*a, 2*b+1) += J2*dNdy[a]*dNdx[b]; // K1 dk(2*a+1, 2*b) += J2*dNdx[a]*dNdy[b]; // K1 dk(2*a+1, 2*b+1) += J2*(2*dNdy[a]*dNdy[b] + dNdx[a]*dNdx[b]); // K1 // K(2*a, 2*b) += lambda*J2*dNdx[a]*dNdx[b]; // K2 // K(2*a, 2*b+1) += lambda*J2*dNdx[a]*dNdy[b]; // K2 // K(2*a+1, 2*b) += lambda*J2*dNdy[a]*dNdx[b]; // K2 // K(2*a+1, 2*b+1) += lambda*J2*dNdy[a]*dNdy[b]; // K2 } } }
void PFEMElement2DBubble::getdF(Matrix& df) const { df.resize(6,6); df.Zero(); for(int a=0; a<3; a++) { for(int b=0; b<6; b++) { df(2*a,b) = bx*dJ(b); df(2*a+1,b) = by*dJ(b); } } df *= rho*thickness/6.0; // velocity if(mu > 0) { Vector v(6); for(int a=0; a<3; a++) { const Vector& vel = nodes[2*a]->getTrialVel(); v(2*a) = vel(0); v(2*a+1) = vel(1); } Matrix dk(6,6); getdK(v,dk); df -= dk; } }
void PFEMElement2DBubble::getdFbub(Matrix& dfb) const { dfb.resize(2,6); dfb.Zero(); for(int b=0; b<6; b++) { dfb(0,b) = bx*dJ(b); dfb(1,b) = by*dJ(b); } dfb *= rho*thickness*27.0/120.0; }
void PFEMElement2DBubble::getKbub(Matrix& kbub) const { kbub.resize(2,2); kbub.Zero(); double dNdx[3], dNdy[3]; for(int a=0; a<3; a++) { dNdx[a] = dJ(2*a)/J; dNdy[a] = dJ(2*a+1)/J; } for(int a=0; a<3; a++) { kbub(0,0) += 81*mu*J*thickness/40.0*(2*dNdx[a]*dNdx[a]+dNdy[a]*dNdy[a]); kbub(0,1) += 81*mu*J*thickness/40.0*(dNdx[a]*dNdy[a]); kbub(1,0) += 81*mu*J*thickness/40.0*(dNdx[a]*dNdy[a]); kbub(1,1) += 81*mu*J*thickness/40.0*(dNdx[a]*dNdx[a]+2*dNdy[a]*dNdy[a]); } }
void PFEMElement2DBubble::getGbub(Matrix& gbub) const { gbub.resize(2,3); for(int a=0; a<2; a++) { for(int b=0; b<3; b++) { gbub(a,b) = dJ(2*b+a); } } gbub *= -27.0*thickness/120.0; }
void PFEMElement2DBubble::getdinvMbub(const Vector& vb, Matrix& dmb) const { dmb.resize(2,6); dmb.Zero(); for(int a=0; a<2; a++) { for(int b=0; b<6; b++) { dmb(a,b) = vb(a)*dJ(b); } } dmb *= -5040.0*ops_Dt/(1863.0*thickness*rho*J*J); }
// geometric sensitivity void PFEMElement2DBubble::getdM(const Vector& vdot, Matrix& dm) const { dm.resize(6,6); dm.Zero(); for(int a=0; a<6; a++) { for(int b=0; b<6; b++) { dm(a,b) = vdot(a)*dJ(b); } } dm *= (1.0/6.0+3.0/40.0)*rho*thickness; }
void PFEMElement2DBubble::getdK(const Vector& v, Matrix& dk) const { dk.resize(6,6); getK(dk); dk *= -1.0/J; Vector kv = dk*v; dk.Zero(); for(int a=0; a<6; a++) { for(int b=0; b<6; b++) { dk(a,b) = kv(a)*dJ(b); } } }
void TransformNDOF::GetHessian(const scalarArray &q, se3DbAry &H) { se3Array J(m_iDOF), dJ(m_iDOF); GetJacobian(q, J); scalarArray dq(m_iDOF); for ( int i = 0; i < m_iDOF; i++ ) dq[i] = q[i]; for ( int i = 0; i < m_iDOF; i++ ) { dq[i] += m_rEPS; GetJacobian(dq, dJ); for ( int j = i + 1; j < m_iDOF; j++ ) H[i][j] = (SCALAR_1 / m_rEPS) * (dJ[j] - J[j]); dq[i] -= m_rEPS; } }
float* train_logistic_regression(int n, int d, float *X, int *y, float lam, float *wb) { int i,j; float * dwb = (float*) malloc(sizeof(float)*(d+1)); for(i=0;i<d+1;i++) wb[i]=0.0f; for(i=0;i<10000;i++){ //iter dJ(n,d,X,y,lam,wb,dwb); for(j=0;j<d+1;j++){ wb[j] -= dwb[j]*0.01; } //printf("%f, %f\n",J(n,d,X,y,lam,wb), dwb[0]+dwb[1]+dwb[2]); } free(dwb); for(i=0;i<d+1;i++){ wb[i] = (int)(1000.0f*wb[i]); wb[i] = wb[i]/1000.0f; } return wb; }
void MovementSolute::secondary_flow (const Geometry& geo, const std::vector<double>& Theta_old, const std::vector<double>& Theta_new, const std::vector<double>& q, const symbol name, const std::vector<double>& S, const std::map<size_t, double>& J_forced, const std::map<size_t, double>& C_border, std::vector<double>& M, std::vector<double>& J, const double dt, Treelog& msg) { const size_t cell_size = geo.cell_size (); const size_t edge_size = geo.edge_size (); // Full timstep left. daisy_assert (dt > 0.0); double time_left = dt; // Initial water content. std::vector<double> Theta (cell_size); for (size_t c = 0; c < cell_size; c++) Theta[c] = Theta_old[c]; // Small timesteps. for (;;) { // Are we done yet? const double min_timestep_factor = 1e-19; if (time_left < 0.1 * min_timestep_factor * dt) break; // Find new timestep. double ddt = time_left; // Limit timestep based on water flux. for (size_t e = 0; e < edge_size; e++) { const int cell = (q[e] > 0.0 ? geo.edge_from (e) : geo.edge_to (e)); if (geo.cell_is_internal (cell) && Theta[cell] > 1e-6 && M[cell] > 0.0) { const double loss_rate = std::fabs (q[e]) * geo.edge_area (e); const double content = Theta[cell] * geo.cell_volume (cell); const double time_to_empty = content / loss_rate; if (time_to_empty < min_timestep_factor * dt) { msg.warning ("Too fast water movement in secondary domain"); ddt = min_timestep_factor * dt; break; } // Go down in timestep while it takes less than two to empty cell. while (time_to_empty < 2.0 * ddt) ddt *= 0.5; } } // Cell source. Must be before transport to avoid negative values. for (size_t c = 0; c < cell_size; c++) M[c] += S[c] * ddt; // Find fluxes using new values (more stable). std::vector<double> dJ (edge_size, -42.42e42); for (size_t e = 0; e < edge_size; e++) { std::map<size_t, double>::const_iterator i = J_forced.find (e); if (i != J_forced.end ()) // Forced flux. { dJ[e] = (*i).second; daisy_assert (std::isfinite (dJ[e])); continue; } const int edge_from = geo.edge_from (e); const int edge_to = geo.edge_to (e); const bool in_flux = q[e] > 0.0; int flux_from = in_flux ? edge_from : edge_to; double C_flux_from = -42.42e42; if (geo.cell_is_internal (flux_from)) // Internal cell, use its concentration. { if (Theta[flux_from] > 1e-6 && M[flux_from] > 0.0) // Positive content in positive water. C_flux_from = M[flux_from] / Theta[flux_from]; else // You can't cut the hair of a bald guy. C_flux_from = 0.0; } else { i = C_border.find (e); if (i != C_border.end ()) // Specified by C_border. C_flux_from = (*i).second; else // Assume no gradient. { const int flux_to = in_flux ? edge_to : edge_from; daisy_assert (geo.cell_is_internal (flux_to)); if (Theta[flux_to] > 1e-6 && M[flux_to] > 0.0) // Positive content in positive water. C_flux_from = M[flux_to] / Theta[flux_to]; else // You can't cut the hair of a bald guy. C_flux_from = 0.0; } } // Convection. daisy_assert (std::isfinite (q[e])); daisy_assert (C_flux_from >= 0.0); dJ[e] = q[e] * C_flux_from; daisy_assert (std::isfinite (dJ[e])); } // Update values for fluxes. for (size_t e = 0; e < edge_size; e++) { const double value = ddt * dJ[e] * geo.edge_area (e); const int from = geo.edge_from (e); if (geo.cell_is_internal (from)) M[from] -= value / geo.cell_volume (from); const int to = geo.edge_to (e); if (geo.cell_is_internal (to)) M[to] += value / geo.cell_volume (to); J[e] += dJ[e] * ddt / dt; } // Update time left. time_left -= ddt; // Interpolate Theta. for (size_t c = 0; c < cell_size; c++) { const double left = time_left / dt; const double done = 1.0 - left; Theta[c] = left * Theta_old[c] + done * Theta_new[c]; } } }
template<typename Robot> void DDP<Robot>::iterate(int const & itr_max, std::vector<U> & us0) { std::vector<MatNM> Ks; std::vector<VecN> kus; Ks.reserve(num); Ks.resize(num, MatNM::Zero()); kus.reserve(num); kus.resize(num, VecN::Zero()); double lambda=params.lambda; double dlambda=params.dlambda; for(int i=0;i<itr_max;i++) { // std::cout<<"========================================================================"<<std::endl; // std::cout<<"Iteration # "<<i<<std::endl; // std::cout<<"------------------------------------------------------------------------"<<std::endl; // backward pass bool backPassDone=false; while(!backPassDone) { int result=backwards(Ks,kus,lambda); if(result>=0) { dlambda=std::max(dlambda*params.lambdaFactor, params.lambdaFactor); lambda=std::max(lambda*dlambda, params.lambdaMin); if(lambda>params.lambdaMax) break; continue; } backPassDone=true; } double gnorm=getGnorm(kus,us); if(gnorm<params.tolGrad && lambda<1e-5) { dlambda=std::min(dlambda/params.lambdaFactor, 1.0/params.lambdaFactor); lambda=lambda*dlambda*(lambda>params.lambdaMin); #ifdef PRINT std::cout<<"SUCCESS: gradient norm = "<<gnorm" < tolGrad"<<std::endl #endif break; } // forward pass bool fwdPassDone=false; std::vector<State> xns; std::vector<U> uns; double Jn; double actual; double expected; xns.reserve(num+1); uns.reserve(num); xns.resize(num+1,x0); uns.resize(num,VecN::Zero()); if(backPassDone) { double alpha=params.alpha; while(alpha>params.alphaMin) { forwards(Ks, kus, alpha, xns, uns, Jn); actual=J0-Jn; expected=-alpha*dJ(0)-alpha*alpha*dJ(1); double reductionRatio=-1; if(expected>0) reductionRatio=actual/expected; // else // std::cout<<"WARNING: non-positive expected reduction: should not occur"<<std::endl; if(reductionRatio>params.reductionRatioMin) fwdPassDone=true; break; alpha*=params.dalphaFactor; } } // std::cout<<"--------------------------------------------"<<std::endl; // std::cout<<"Results"<<std::endl; // std::cout<<"--------------------------------------------"<<std::endl; if(fwdPassDone) { dlambda=std::min(dlambda/params.lambdaFactor, 1.0/params.lambdaFactor); lambda=lambda*dlambda*(lambda>params.lambdaMin); // std::cout<<"Improved"<<std::endl; // std::cout<<"lambda: "<<lambda<<std::endl; // std::cout<<"dlambda: "<<dlambda<<std::endl; // std::cout<<"Jn: "<<Jn<<std::endl; // std::cout<<"Jn: "<<Jn<<std::endl; // std::cout<<Robot::State::diff(xns[num],xrefs[num]).transpose()<<std::endl; xs=xns; us=uns; J0=Jn; if(actual<params.tolFun) { #ifdef PRINT std::cout<<"SUCCESS: cost change = "<<actual<<" < tolFun"<<std::endl; #endif break; } } else { dlambda=std::max(dlambda*params.lambdaFactor, params.lambdaFactor); lambda=std::max(lambda*dlambda, params.lambdaMin); // std::cout<<"No step found"<<std::endl; // std::cout<<"lambda: "<<lambda<<std::endl; // std::cout<<"dlambda: "<<dlambda<<std::endl; // std::cout<<"Jn: "<<Jn<<std::endl; if (lambda>params.lambdaMax) break; } // std::cout<<"========================================================================"<<std::endl<<std::endl; }