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