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