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
}
// 获取节点的值
CString CVoucherFile::_GetNodeText(MSXML2::IXMLDOMNodePtr pNode)
{
CString strNodeText;
if(pNode != NULL) strNodeText = V2T(pNode->text);
return strNodeText;
}
