void block(DynamicSparseMatrix<double>& container, int start_row, int start_col, DynamicSparseMatrix<double>& data) {
for (int k = 0; k < data.outerSize(); ++k) {
for(DynamicSparseMatrix<double>::InnerIterator it(data,k); it; ++it) {
container.coeffRef(start_row+it.row(), start_col+it.col()) = it.value();
}
}
}
bool test_random_setter(DynamicSparseMatrix<T>& sm, const DenseType& ref, const std::vector<Vector2i>& nonzeroCoords)
{
sm.setZero();
std::vector<Vector2i> remaining = nonzeroCoords;
while(!remaining.empty())
{
int i = internal::random<int>(0,static_cast<int>(remaining.size())-1);
sm.coeffRef(remaining[i].x(),remaining[i].y()) = ref.coeff(remaining[i].x(),remaining[i].y());
remaining[i] = remaining.back();
remaining.pop_back();
}
return sm.isApprox(ref);
}
void WorldSQP::compute_difference_block(DynamicSparseMatrix<double>& m)
{
m.resize(_size_each_state*(_num_worlds-1), _size_each_state*(_num_worlds-2));
for (int i = 0; i < _num_worlds-1; i++) {
int num_rows_start = i*_size_each_state;
for (int r = 0; r < _size_each_state; r++) {
if (i != 0)
m.coeffRef(num_rows_start+r,num_rows_start+r - _size_each_state) = 1;
if (i != _num_worlds-2)
m.coeffRef(num_rows_start+r,num_rows_start+r) = -1;
}
}
}
void block(DynamicSparseMatrix<double>& container, int start_row, int start_col, MatrixXd& data) {
for (int i = 0; i < data.rows(); i++) {
for (int j = 0; j < data.cols(); j++) {
container.coeffRef(start_row+i, start_col+j) = data(i,j);
}
}
}
void WorldSQP::compute_all_jacobians()
{
boost::thread_group group;
for (int traj_ind = 0; traj_ind < _num_traj; traj_ind++) {
cout << "Computing Jacobians on traj = " << traj_ind << endl; ;
for (int i=0; i < current_states[traj_ind].size()-1; i++)
{
if (current_jacobians[traj_ind][i].rows() == 0) {
group.create_thread( boost::bind(computeJacobian,
current_states[traj_ind][i],
¤t_jacobians[traj_ind][i]) );
}
}
}
group.join_all();
for (int traj_ind = 0; traj_ind < _num_traj; traj_ind++) {
DynamicSparseMatrix<double> J;
J.resize((_num_worlds-1)*_size_each_state, (_num_worlds-1)*_size_each_control);
for (int i = 0; i < current_states[traj_ind].size()-1; i++)
{
int num_rows_start = i*_size_each_state;
int num_cols_start = i*_size_each_control;
for (int r=0; r < _size_each_state; r++)
{
for (int c=0; c < _size_each_control; c++)
{
J.coeffRef(num_rows_start+r, num_cols_start+c) = current_jacobians[traj_ind][i](r,c);
}
}
//++progress;
}
block(_all_trans,traj_ind*_size_each_state*(_num_worlds-1), _cols_all_unknown_states, J);
}
}
