Eigen::SparseMatrix<double> Condi2Joint(Eigen::SparseMatrix<double> Condi, Eigen::SparseVector<double> Pa)
{	// second dimension of Condi is the parent
	Eigen::SparseMatrix<double> Joint;
	Joint.resize(Condi.rows(), Condi.cols());

	for (int cols = 0; cols < Condi.cols(); cols++)
	{
		Eigen::SparseVector<double> tmp_vec = Condi.block(0, cols, Condi.rows(), 1)*Pa.coeff(cols);
		for (int id_rows = 0; id_rows < tmp_vec.size(); id_rows++)
		{
			Joint.coeffRef(id_rows, cols) = tmp_vec.coeff(id_rows);
		}

	}
	Joint.prune(TOLERANCE);
	return Joint;

}
Eigen::SparseVector<double> normProbVector(Eigen::SparseVector<double> P_vec)
{
	VectorXd P_dense_vec = (VectorXd)P_vec;
	Eigen::SparseVector<double> P_norm;
	if (P_dense_vec == VectorXd::Zero(P_vec.size()))
	{
		P_norm = P_vec;
	}
	else{
		double P_positive = 0; double P_negative = 0;

		for (int row_idx = 0; row_idx < P_vec.size(); row_idx++){
			P_positive = (P_vec.coeff(row_idx) > 0) ? (P_positive + P_vec.coeff(row_idx)) : P_positive;
			P_negative = (P_vec.coeff(row_idx) > 0) ? P_negative : (P_negative + P_vec.coeff(row_idx));
		}
		if (fabs(P_positive) < fabs(P_negative)){
			P_norm = -P_vec / fabs(P_negative);
		}
		else{
			P_norm = P_vec / fabs(P_positive);
		}

		for (int row_idx = 0; row_idx < P_vec.size(); row_idx++){
			P_norm.coeffRef(row_idx) = (P_norm.coeff(row_idx)<0) ? 0 : P_norm.coeff(row_idx);
		}
	}
	P_norm.prune(TOLERANCE);
	return P_norm;
}
Ejemplo n.º 3
0
Eigen::SparseVector<double> pinv_vector(Eigen::SparseVector<double> pinvvec)
{
	Eigen::SparseVector<double> singularValues_inv;
	singularValues_inv.resize(pinvvec.size());

	for (int i = 0; i<pinvvec.size(); ++i) {
		singularValues_inv.coeffRef(i) = (fabs(pinvvec.coeff(i)) > TOLERANCE) ? 1.0 / pinvvec.coeff(i) : 0;
	}
	singularValues_inv.prune(TOLERANCE);
	return singularValues_inv;
}
void compare(const Physika::VectorND<mytype> &a, const Eigen::SparseVector<mytype> &b)
{
    for (unsigned int i = 0; i < a.dims(); ++i)
    {
        if (a[i] != b.coeff(i))
        {
            cout << "uncorrectly vector multiply a sparsematrix" << endl;
            return;
        }
    }
    cout << "correctness OK!" << endl;
    return ;
}
TEST_F(MassSpringConstraintFixedPointTest, BuildMlcpIndiciesTest)
{
	auto implementation = std::make_shared<MassSpringConstraintFixedPoint>();

	MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(11, 6, 3);

	// Suppose 5 dof and 1 constraint are defined elsewhere.  Then H, CHt, HCHt, and b are prebuilt.
	Eigen::SparseVector<double, Eigen::RowMajor, ptrdiff_t> localH;
	localH.resize(5);
	localH.reserve(5);
	localH.insert(0) = 0.9478;
	localH.insert(1) = -0.3807;
	localH.insert(2) = 0.5536;
	localH.insert(3) = -0.6944;
	localH.insert(4) = 0.1815;

	mlcpPhysicsProblem.H.coeffRef(0, 0) += localH.coeff(0);
	mlcpPhysicsProblem.H.coeffRef(0, 1) += localH.coeff(1);
	mlcpPhysicsProblem.H.coeffRef(0, 2) += localH.coeff(2);
	mlcpPhysicsProblem.H.coeffRef(0, 3) += localH.coeff(3);
	mlcpPhysicsProblem.H.coeffRef(0, 4) += localH.coeff(4);

	Eigen::Matrix<double, 5, 5> localC;
	localC <<
		   -0.2294,  0.5160,  0.2520,  0.5941, -0.4854,
		   0.1233, -0.4433,  0.3679,  0.9307,  0.2600,
		   0.1988,  0.6637, -0.7591,  0.1475,  0.8517,
		   -0.5495, -0.4305,  0.3162, -0.7862,  0.7627,
		   -0.5754,  0.4108,  0.8445, -0.5565,  0.7150;
	localC = localC * localC.transpose(); // force to be symmetric

	Eigen::Matrix<double, 5, 1> localCHt = localC * localH.transpose();
	mlcpPhysicsProblem.CHt.block<5, 1>(0, 0) = localCHt;

	mlcpPhysicsProblem.A.block<1, 1>(0, 0) = localH * localCHt;

	mlcpPhysicsProblem.b.block<1, 1>(0, 0)[0] = 0.6991;

	// Place mass-spring at 5th dof and 1th constraint (0-based).
	size_t indexOfRepresentation = 5;
	size_t indexOfConstraint = 1;

	setConstraintAtNode(1);

	implementation->build(dt, m_constraintData, m_localization,
		&mlcpPhysicsProblem, indexOfRepresentation, indexOfConstraint, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);

	// b -> E -> [#constraints, 1]
	const Vector3d newPosition = m_extremities[1] - Vector3d::UnitY() * 9.81 * dt * dt;
	EXPECT_TRUE(mlcpPhysicsProblem.b.segment<3>(indexOfConstraint).isApprox(newPosition));

	// H -> [#constraints, #dof]
	SurgSim::Math::Matrix H = mlcpPhysicsProblem.H;
	SurgSim::Math::Matrix expectedH = SurgSim::Math::Matrix::Zero(3, 6);
	expectedH.block<3, 3>(0, 3) = SurgSim::Math::Matrix33d::Identity() * dt;
	EXPECT_TRUE((H.block<3, 6>(indexOfConstraint, indexOfRepresentation).isApprox(expectedH))) << H;

	// C -> [#dof, #dof]
	// CHt -> [#dof, #constraints]
	SurgSim::Math::Matrix CHt = mlcpPhysicsProblem.CHt;
	SurgSim::Math::Matrix expectedCHt = SurgSim::Math::Matrix::Zero(6, 3);
	expectedCHt.block<3, 3>(3, 0) = SurgSim::Math::Matrix33d::Identity() * dt * dt / m_massPerNode;
	EXPECT_TRUE((CHt.block(indexOfRepresentation, indexOfConstraint, 6, 3).isApprox(expectedCHt))) << CHt;

	// A -> HCHt -> [#constraints, #constraints]
	SurgSim::Math::Matrix expectedA = dt * dt * dt / m_massPerNode * SurgSim::Math::Matrix33d::Identity();
	EXPECT_TRUE(mlcpPhysicsProblem.A.block(1, 1, 3, 3).isApprox(expectedA));
}