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> 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;
}
IGL_INLINE void igl::diag(
const Eigen::SparseVector<T>& V,
Eigen::SparseMatrix<T>& X)
{
// clear and resize output
Eigen::DynamicSparseMatrix<T, Eigen::RowMajor> dyn_X(V.size(),V.size());
dyn_X.reserve(V.size());
// loop over non-zeros
for(typename Eigen::SparseVector<T>::InnerIterator it(V); it; ++it)
{
dyn_X.coeffRef(it.index(),it.index()) += it.value();
}
X = Eigen::SparseMatrix<T>(dyn_X);
}
void Scene::waveletLight()
{
// Try and cast the active light as a cubemap. If that fails,
// then you can't light the scene using that current cubemap.
CubeMap* cubeMap = getActiveCubemap() ;
if( !cubeMap )
{
error( "I can't wavelet light without a cubemap" ) ;
return ;
}
for( int i = 0 ; i < window->scene->shapes.size() ; i++ )
{
Shape *shape = shapes[ i ] ;
for( int j = 0 ; j < shape->meshGroup->meshes.size() ; j++ )
{
Mesh *mesh = shape->meshGroup->meshes[j];
for( int k = 0 ; k < mesh->verts.size() ; k++ )
{
Vector totalLight ;
AllVertex& vertex = mesh->verts[ k ] ;
/// The next line doesn't work due to an Eigen bug
///Vector totalLight = vertex.cubeMap->sparseTransformedColor.cwiseProduct( scene->cubeMap->sparseTransformedColor ).sum() ; // EIGENBUG: This doesn't work. You must save it in a temporary, as done below
////if( !vertex.cubeMap->sparseTransformedColor.nonZeros() )
//// error( "vertex cubemap empty" ) ;
////if( !cubeMap->sparseTransformedColor.nonZeros() )
//// error( "cubemap empty" ) ;
// This is a bottleneck.
Eigen::SparseVector<Vector> v = vertex.cubeMap->sparseTransformedColor.cwiseProduct( cubeMap->sparseTransformedColor );
totalLight = v.sum() ;
//totalLight = vertex.cubeMap->sparseM.dot( scene->cubeMap->sparseM ) ;
// my ability to reflect limited by my diffuse material, and assign the color to the wavelet color
vertex.color[ ColorIndex::WaveletComputed ] = totalLight * vertex.color[ ColorIndex::DiffuseMaterial ] ;
}//each vertex
}//each mesh
}//each shape
}
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;
}
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 ;
}
IGL_INLINE void igl::sum(
const Eigen::SparseMatrix<T>& X,
const int dim,
Eigen::SparseVector<T>& S)
{
// dim must be 2 or 1
assert(dim == 1 || dim == 2);
// Get size of input
int m = X.rows();
int n = X.cols();
// resize output
if(dim==1)
{
S = Eigen::SparseVector<T>(n);
}else
{
S = Eigen::SparseVector<T>(m);
}
// Iterate over outside
for(int k=0; k<X.outerSize(); ++k)
{
// Iterate over inside
for(typename Eigen::SparseMatrix<T>::InnerIterator it (X,k); it; ++it)
{
if(dim == 1)
{
S.coeffRef(it.col()) += it.value();
}else
{
S.coeffRef(it.row()) += it.value();
}
}
}
}
IGL_INLINE void igl::find(
const Eigen::SparseVector<T>& X,
Eigen::Matrix<int,Eigen::Dynamic,1> & I,
Eigen::Matrix<T,Eigen::Dynamic,1> & V)
{
// Resize outputs to fit nonzeros
I.resize(X.nonZeros());
V.resize(X.nonZeros());
int i = 0;
// loop over non-zeros
for(typename Eigen::SparseVector<T>::InnerIterator it(X); it; ++it)
{
I(i) = it.index();
V(i) = it.value();
i++;
}
}
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));
}