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