bool pre_draw(igl::opengl::glfw::Viewer & viewer)
{
using namespace Eigen;
using namespace std;
if(viewer.core.is_animating)
{
// Interpolate pose and identity
RotationList anim_pose(pose.size());
for(int e = 0;e<pose.size();e++)
{
anim_pose[e] = pose[e].slerp(anim_t,Quaterniond::Identity());
}
// Propagate relative rotations via FK to retrieve absolute transformations
RotationList vQ;
vector<Vector3d> vT;
igl::forward_kinematics(C,BE,P,anim_pose,vQ,vT);
const int dim = C.cols();
MatrixXd T(BE.rows()*(dim+1),dim);
for(int e = 0;e<BE.rows();e++)
{
Affine3d a = Affine3d::Identity();
a.translate(vT[e]);
a.rotate(vQ[e]);
T.block(e*(dim+1),0,dim+1,dim) =
a.matrix().transpose().block(0,0,dim+1,dim);
}
// Compute deformation via LBS as matrix multiplication
U = M*T;
// Also deform skeleton edges
MatrixXd CT;
MatrixXi BET;
igl::deform_skeleton(C,BE,T,CT,BET);
viewer.data().set_vertices(U);
viewer.data().set_edges(CT,BET,sea_green);
viewer.data().compute_normals();
anim_t += anim_t_dir;
anim_t_dir *= (anim_t>=1.0 || anim_t<=0.0?-1.0:1.0);
}
return false;
}
void CCorrelationFilters::fusion_matrix_inverse(Eigen::ArrayXXcd &X, Eigen::MatrixXi indices)
{
/* We compute the inverse of the fusion matrix by using the schur complement
* on our data structure. We use an inplace implementation therefore it is quite
* memory efficient as well.
*/
int num_blocks=0;
int total_blocks=0;
int size_blocks = X.rows();
if (indices.rows() != indices.cols()){
std::cout << "Something Wrong" << std::endl;
return;
}
else
{
num_blocks = indices.rows();
}
if (num_blocks == 1){
X = X.inverse();
return;
}
if (num_blocks == 2)
{
Eigen::ArrayXXcd temp1;
Eigen::ArrayXXcd temp2;
Eigen::ArrayXXcd DC(size_blocks,1);
Eigen::ArrayXXcd BD(size_blocks,1);
Eigen::ArrayXXcd BDC(size_blocks,1);
Eigen::ArrayXXcd ABDC(size_blocks,1);
DC = X.block(0,indices(1,0),size_blocks,1)/X.block(0,indices(1,1),size_blocks,1);
BD = X.block(0,indices(0,1),size_blocks,1)/X.block(0,indices(1,1),size_blocks,1);
BDC = X.block(0,indices(0,1),size_blocks,1)*DC;
ABDC = (X.block(0,indices(0,0),size_blocks,1)-BDC).inverse();
X.block(0,indices(0,0),size_blocks,1) = ABDC;
X.block(0,indices(0,1),size_blocks,1) = -ABDC*BD;
X.block(0,indices(1,0),size_blocks,1) = -DC*ABDC;
X.block(0,indices(1,1),size_blocks,1) = 1/X.block(0,indices(1,1),size_blocks,1) - X.block(0,indices(1,0),size_blocks,1)*BD;
}
else
{
int num_B = num_blocks-1;
int num_C = num_blocks-1;
Eigen::MatrixXi ind_A;
Eigen::MatrixXi ind_B;
Eigen::MatrixXi ind_C;
Eigen::MatrixXi ind_D;
total_blocks = num_blocks*num_blocks;
ind_D = indices.block(num_blocks-1,num_blocks-1,1,1);
ind_B = indices.block(0,num_blocks-1,num_blocks-1,1);
ind_C = indices.block(num_blocks-1,0,1,num_blocks-1);
ind_A = indices.block(0,0,num_blocks-1,num_blocks-1).transpose();
num_blocks = num_blocks-1;
Eigen::ArrayXXcd D = Eigen::ArrayXXcd::Zero(size_blocks,1);
Eigen::ArrayXXcd DC = Eigen::ArrayXXcd::Zero(size_blocks,num_blocks);
Eigen::ArrayXXcd BD = Eigen::ArrayXXcd::Zero(size_blocks,num_blocks);
Eigen::ArrayXXcd BDC = Eigen::ArrayXXcd::Zero(size_blocks,num_blocks*num_blocks);
Eigen::ArrayXXcd ABDC = Eigen::ArrayXXcd::Zero(size_blocks,num_blocks*num_blocks);
Eigen::ArrayXXcd tmp_B = Eigen::ArrayXXcd::Zero(size_blocks,num_B);
Eigen::ArrayXXcd tmp_C = Eigen::ArrayXXcd::Zero(size_blocks,num_C);
Eigen::ArrayXXcd tmp_D = Eigen::ArrayXXcd::Zero(size_blocks,1);
for(int i=0;i<num_C;i++){
tmp_C.block(0,i,size_blocks,1) = X.block(0,ind_C(0,i),size_blocks,1);
}
for(int i=0;i<num_B;i++){
tmp_B.block(0,i,size_blocks,1) = X.block(0,ind_B(i,0),size_blocks,1);
}
D.block(0,0,size_blocks,1) = X.block(0,total_blocks-1,size_blocks,1).inverse();
Eigen::Vector2i num_blocks_1, num_blocks_2;
num_blocks_1 << 1,1;
num_blocks_2 << 1,num_blocks;
fusion_matrix_multiply(DC,D,tmp_C,num_blocks_1,num_blocks_2);
num_blocks_1 << num_blocks,1;
num_blocks_2 << 1,1;
fusion_matrix_multiply(BD,tmp_B,D,num_blocks_1,num_blocks_2);
num_blocks_1 << num_blocks,1;
num_blocks_2 << 1,num_blocks;
fusion_matrix_multiply(BDC,tmp_B,DC,num_blocks_1,num_blocks_2);
for(int i=0;i<ind_A.rows();i++){
for(int j=0;j<ind_A.cols();j++){
X.block(0,ind_A(i,j),size_blocks,1) -= BDC.block(0,i,size_blocks,1);
}
}
fusion_matrix_inverse(X,ind_A);
tmp_B = Eigen::ArrayXXcd::Zero(size_blocks,num_B);
tmp_C = Eigen::ArrayXXcd::Zero(size_blocks,num_C);
tmp_D = Eigen::ArrayXXcd::Zero(size_blocks,1);
num_blocks_1 << num_blocks,num_blocks;
num_blocks_2 << num_blocks,1;
fusion_matrix_multiply(tmp_B,ABDC,BD,num_blocks_1,num_blocks_2);
for(int i=0;i<num_B;i++){
X.block(0,ind_B(i,0),size_blocks,1) = -tmp_B.block(0,i,size_blocks,1);
}
num_blocks_1 << 1,num_blocks;
num_blocks_2 << num_blocks,num_blocks;
fusion_matrix_multiply(tmp_C,DC,ABDC,num_blocks_1,num_blocks_2);
for(int i=0;i<num_C;i++){
X.block(0,ind_C(0,i),size_blocks,1) = -tmp_C.block(0,i,size_blocks,1);
}
tmp_C = Eigen::ArrayXXcd::Zero(size_blocks,num_C);
num_blocks_1 << 1,num_blocks;
num_blocks_2 << num_blocks,num_blocks;
fusion_matrix_multiply(tmp_C,DC,ABDC,num_blocks_1,num_blocks_2);
num_blocks_1 << 1,num_blocks;
num_blocks_2 << num_blocks,1;
fusion_matrix_multiply(tmp_D,tmp_C,BD,num_blocks_1,num_blocks_2);
tmp_D += D;
X.block(0,total_blocks-1,size_blocks,1) = tmp_D.block(0,0,size_blocks,1);
}
}
