void PFEMElement2DBubble::getdFp(Matrix& dfp) const { Matrix gb(2,3); getGbub(gb); Vector fb(2); getFbub(fb); double invmb = getinvMbub(); getdGbt(fb*invmb, dfp); Matrix dmb(2,6); getdinvMbub(fb,dmb); dfp.addMatrixTransposeProduct(-1.0, gb, dmb, -1.0); Matrix dfb(2,6); getdFbub(dfb); dfp.addMatrixTransposeProduct(1.0, gb, dfb, -invmb); }
void PFEMElement2DBubble::getdFp(Vector& dfp) const { Matrix gb(2,3); getGbub(gb); double invmb = getinvMbub(); Vector fb(2); getFbub(fb); double dinvmb = getdinvMbub(); Vector dfb(2); getdFbub(dfb); // bubble force dfp.resize(3); dfp.Zero(); dfp.addMatrixTransposeVector(0.0, gb, fb, -dinvmb); dfp.addMatrixTransposeVector(1.0, gb, dfb, -invmb); }
double IK::solve_ik() { int i, j; double current_max_condnum = -1.0; copy_jacobian(); if(n_all_const > 0) { //// // check rank when HIGH_IF_POSSIBLE constraints have high priority int n_high_const; fMat Jhigh; fMat wJhigh; fVec fb_high; fVec weight_high; int* is_high_const = 0; double cond_number = 1.0; // cerr << "---" << endl; // cerr << n_const[HIGH_PRIORITY] << " " << n_const[HIGH_IF_POSSIBLE] << " " << n_const[LOW_PRIORITY] << endl; if(n_const[HIGH_IF_POSSIBLE] > 0) { is_high_const = new int [n_const[HIGH_IF_POSSIBLE]]; // initialize for(i=0; i<n_const[HIGH_IF_POSSIBLE]; i++) is_high_const[i] = true; // search int search_phase = 0; while(1) { n_high_const = n_const[HIGH_PRIORITY]; for(i=0; i<n_const[HIGH_IF_POSSIBLE]; i++) { if(is_high_const[i]) n_high_const++; } Jhigh.resize(n_high_const, n_dof); wJhigh.resize(n_high_const, n_dof); fb_high.resize(n_high_const); weight_high.resize(n_high_const); if(n_const[HIGH_PRIORITY] > 0) { // set fb and J of higher priority pins for(i=0; i<n_const[HIGH_PRIORITY]; i++) { fb_high(i) = fb[HIGH_PRIORITY](i); weight_high(i) = weight[HIGH_PRIORITY](i); for(j=0; j<n_dof; j++) { Jhigh(i, j) = J[HIGH_PRIORITY](i, j); wJhigh(i, j) = Jhigh(i, j) * weight_high(i) / joint_weights(j); } } } int count = 0; // set fb and J of medium priority pins for(i=0; i<n_const[HIGH_IF_POSSIBLE]; i++) { if(is_high_const[i]) { fb_high(n_const[HIGH_PRIORITY]+count) = fb[HIGH_IF_POSSIBLE](i); weight_high(n_const[HIGH_PRIORITY]+count) = weight[HIGH_IF_POSSIBLE](i); for(j=0; j<n_dof; j++) { Jhigh(n_const[HIGH_PRIORITY]+count, j) = J[HIGH_IF_POSSIBLE](i, j); wJhigh(n_const[HIGH_PRIORITY]+count, j) = J[HIGH_IF_POSSIBLE](i, j) * weight[HIGH_IF_POSSIBLE](i) / joint_weights(j); } count++; } } // singular value decomposition fMat U(n_high_const, n_high_const), VT(n_dof, n_dof); fVec s; int s_size; if(n_high_const < n_dof) s_size = n_high_const; else s_size = n_dof; s.resize(s_size); wJhigh.svd(U, s, VT); double condnum_limit = max_condnum * 100.0; if(s(s_size-1) > s(0)/(max_condnum*condnum_limit)) cond_number = s(0) / s(s_size-1); else cond_number = condnum_limit; if(current_max_condnum < 0.0 || cond_number > current_max_condnum) { current_max_condnum = cond_number; } if(n_high_const <= n_const[HIGH_PRIORITY]) break; // remove some constraints if(cond_number > max_condnum) { int reduced = false; for(i=n_constraints-1; i>=0; i--) { if(constraints[i]->enabled && constraints[i]->priority == HIGH_IF_POSSIBLE && constraints[i]->i_const >= 0 && constraints[i]->GetType() == HANDLE_CONSTRAINT && is_high_const[constraints[i]->i_const]) { IKHandle* h = (IKHandle*)constraints[i]; if(search_phase || (!search_phase && h->joint->DescendantDOF() > 0)) { for(j=0; j<constraints[i]->n_const; j++) { is_high_const[constraints[i]->i_const + j] = false; } constraints[i]->is_dropped = true; // cerr << "r" << flush; reduced = true; break; } } } if(!reduced) search_phase++; } else break; } } else { n_high_const = n_const[HIGH_PRIORITY]; Jhigh.resize(n_high_const, n_dof); wJhigh.resize(n_high_const, n_dof); fb_high.resize(n_high_const); weight_high.resize(n_high_const); if(n_high_const > 0) { Jhigh.set(J[HIGH_PRIORITY]); fb_high.set(fb[HIGH_PRIORITY]); weight_high.set(weight[HIGH_PRIORITY]); } } #if 0 //// // adjust feedback according to the condition number if(current_max_condnum > max_condnum) fb_high.zero(); else { double k = (current_max_condnum-max_condnum)/(1.0-max_condnum); fb_high *= k; cerr << current_max_condnum << ", " << k << endl; } //// //// if(current_max_condnum < 0.0) current_max_condnum = 1.0; #endif int n_low_const = n_all_const - n_high_const; int low_first = 0, count = 0; fMat Jlow(n_low_const, n_dof); fVec fb_low(n_low_const); fVec weight_low(n_low_const); for(i=0; i<n_const[HIGH_IF_POSSIBLE]; i++) { if(!is_high_const[i]) { fb_low(count) = fb[HIGH_IF_POSSIBLE](i); weight_low(count) = weight[HIGH_IF_POSSIBLE](i); for(j=0; j<n_dof; j++) { Jlow(count, j) = J[HIGH_IF_POSSIBLE](i, j); } count++; } } low_first = count; double* p = fb_low.data() + low_first; double* q = fb[LOW_PRIORITY].data(); double* r = weight_low.data() + low_first; double* s = weight[LOW_PRIORITY].data(); for(i=0; i<n_const[LOW_PRIORITY]; p++, q++, r++, s++, i++) { // fb_low(low_first+i) = fb[LOW_PRIORITY](i); *p = *q; *r = *s; double* a = Jlow.data() + low_first + i; int a_row = Jlow.row(); double* b = J[LOW_PRIORITY].data() + i; int b_row = J[LOW_PRIORITY].row(); for(j=0; j<n_dof; a+=a_row, b+=b_row, j++) { // Jlow(low_first+i, j) = J[LOW_PRIORITY](i, j); *a = *b; } } if(is_high_const) delete[] is_high_const; fVec jvel(n_dof); // ³û¼â1¡¦x fVec jvel0(n_dof); // ¹ó/¡¦¾å fVec fb_low_0(n_low_const), dfb(n_low_const), y(n_dof); fMat Jinv(n_dof, n_high_const), W(n_dof, n_dof), JW(n_low_const, n_dof); fVec w_error(n_low_const), w_norm(n_dof); // weighted double damping = 0.1; // w_error = 1.0; w_error.set(weight_low); w_norm = 1.0; // w_norm.set(joint_weights); // cerr << "joint_weights = " << joint_weights << endl; // cerr << "weight_high = " << weight_high << endl; // cerr << "weight_low = " << weight_low << endl; #ifdef MEASURE_TIME clock_t t1 = clock(); #endif // ¹ó/¡¦¾å¡¦€ÅæéòÉçïàïêvZ if(n_high_const > 0) { for(i=0; i<n_dof; i++) { int a_row = wJhigh.row(); double* a = wJhigh.data() + a_row*i; int b_row = Jhigh.row(); double* b = Jhigh.data() + b_row*i; double* c = joint_weights.data() + i; double* d = weight_high.data(); for(j=0; j<n_high_const; a++, b++, d++, j++) { // wJhigh(j, i) = Jhigh(j, i) / joint_weights(i); *a = *b * *d / *c; } } fVec w_fb_high(fb_high); for(i=0; i<n_high_const; i++) w_fb_high(i) *= weight_high(i); Jinv.p_inv(wJhigh); jvel0.mul(Jinv, w_fb_high); W.mul(Jinv, wJhigh); for(i=0; i<n_dof; i++) { double* w = W.data() + i; double a = joint_weights(i); for(j=0; j<n_dof; w+=n_dof, j++) { if(i==j) *w -= 1.0; *w /= -a; } jvel0(i) /= a; } JW.mul(Jlow, W); } else { jvel0.zero(); JW.set(Jlow); } #ifdef MEASURE_TIME clock_t t2 = clock(); high_constraint_time += (double)(t2-t1)/(double)CLOCKS_PER_SEC; #endif // Ǥ¡¦#x¥¯¥È¥ë¹à¤ê³×Z if(n_low_const > 0) { fb_low_0.mul(Jlow, jvel0); dfb.sub(fb_low, fb_low_0); y.lineq_sr(JW, w_error, w_norm, damping, dfb); if(n_high_const > 0) jvel.mul(W, y); else jvel.set(y); // fVec error(n_low_const); // error = dfb-Jlow*jvel; // cerr << dfb*dfb << "->" << error*error << endl; } else { jvel.zero(); } // ³û¼â1¡¦x jvel += jvel0; #ifdef MEASURE_TIME clock_t t3 = clock(); low_constraint_time += (double)(t3-t2)/(double)CLOCKS_PER_SEC; #endif SetJointVel(jvel); // cerr << fb_high - Jhigh * jvel << endl; } if(current_max_condnum < 0.0) current_max_condnum = 1.0; return current_max_condnum; }