/// Creates a sparse vector from a dense vector
SparseVectorN::SparseVectorN(const VectorN& x)
{
// setup the vector size
_size = x.size();
// determine the nonzero elements
_nelm = 0;
shared_array<bool> nz_elms(new bool[x.size()]);
for (unsigned i=0; i< x.size(); i++)
if (std::fabs(x[i]) < NEAR_ZERO)
{
_nelm++;
nz_elms[i] = true;
}
else
nz_elms[i] = false;
// declare memory
_indices = shared_array<unsigned>(new unsigned[_nelm]);
_data = shared_array<Real>(new Real[_nelm]);
// setup data
for (unsigned i=0, j=0; i< x.size(); i++)
if (nz_elms[i])
{
_indices[j] = i;
_data[j] = x[i];
j++;
}
}
// minimzer scaling modifier
void Minimizer_AlglibLBFGS::set_minimizer_x_scalings(const VectorN& scale_x) {
alglib::real_1d_array scalings(real_1d_array_with_size(scale_x.size()));
VectorN_TO_real_1d_array(scale_x, scalings);
alglib::minlbfgssetscale(m_minimizer_state, scalings);
alglib::minlbfgssetprecscale(m_minimizer_state);
m_scales = scale_x;
};
/// Computes the dot product between a sparse vector and a dense vector
Real SparseVectorN::dot(const VectorN& x) const
{
Real result = 0;
if (x.size() != _size)
throw MissizeException();
for (unsigned i=0; i< _nelm; i++)
result += x[_indices[i]] * _data[i];
return result;
}
/// runs the simulator and updates all transforms
bool step(void* arg)
{
// get the simulator pointer
boost::shared_ptr<Simulator> s = *(boost::shared_ptr<Simulator>*) arg;
// get the generalized coordinates for all bodies in alphabetical order
std::vector<DynamicBodyPtr> bodies = s->get_dynamic_bodies();
std::sort(bodies.begin(), bodies.end(), compbody);
VectorN q;
outfile << s->current_time;
for (unsigned i=0; i< bodies.size(); i++)
{
bodies[i]->get_generalized_coordinates(DynamicBody::eRodrigues, q);
for (unsigned j=0; j< q.size(); j++)
outfile << " " << q[j];
}
outfile << std::endl;
// output the iteration #
if (OUTPUT_ITER_NUM)
std::cout << "iteration: " << ITER << " simulation time: " << s->current_time << std::endl;
if (Log<OutputToFile>::reporting_level > 0)
FILE_LOG(Log<OutputToFile>::reporting_level) << "iteration: " << ITER << " simulation time: " << s->current_time << std::endl;
// step the simulator and update visualization
clock_t pre_sim_t = clock();
s->step(STEP_SIZE);
clock_t post_sim_t = clock();
Real total_t = (post_sim_t - pre_sim_t) / (Real) CLOCKS_PER_SEC;
TOTAL_TIME += total_t;
// output the iteration / stepping rate
if (OUTPUT_SIM_RATE)
std::cout << "time to compute last iteration: " << total_t << " (" << TOTAL_TIME / ITER << "s/iter, " << TOTAL_TIME / s->current_time << "s/step)" << std::endl;
// update the iteration #
ITER++;
// check that maximum number of iterations or maximum time not exceeded
if (ITER >= MAX_ITER || s->current_time > MAX_TIME)
return false;
return true;
}
bool PathLCPSolver::solve_lcp(const MatrixN& MM, const VectorN& q, VectorN& z, double tol)
{
const Real INF = std::numeric_limits<Real>::max();
int i, j, k;
int variables;
shared_array<int> m_i, m_j;
shared_array<double> dq, lb, ub, m_ij, x_end;
MCP_Termination termination;
Output_Printf(Output_Log | Output_Status | Output_Listing, "%s: Standalone-C Link\n", Path_Version());
// We need a sparse representation of the matrix
assert(q.size() == MM.rows());
assert(q.size() == z.size());
// Copy q to dq
variables = q.size();
dq = shared_array<double>(new double[variables+1]);
for (i = 0;i < variables; i++)
dq[i] = q[i];
// Copy q to dq
variables = q.size();
dq = shared_array<double>(new double[variables+1]);
for (i = 0;i < variables; i++)
dq[i] = q[i];
int num_non_zeroes = 0;
for (i = 0;i < variables; i++)
for (j = 0;j < variables; j++)
if (std::fabs(MM(i,j)) > NEAR_ZERO)
num_non_zeroes++;
m_i = shared_array<int>(new int[num_non_zeroes + 1]);
m_j = shared_array<int>(new int[num_non_zeroes + 1]);
m_ij = shared_array<double>(new double[num_non_zeroes + 1]);
k = 0;
for (i = 0;i < variables; i++)
{
for (j = 0;j < variables; j++)
{
if (std::fabs(MM(i,j)) > NEAR_ZERO)
{
m_i[k] = i+1;
m_j[k] = j+1;
m_ij[k] = MM(i,j);
k++;
}
}
}
lb = shared_array<double>(new double[variables+1]);
ub = shared_array<double>(new double[variables+1]);
for (i = 0;i < variables; i++)
{
lb[i] = 0.0;
ub[i] = INF;
}
x_end = shared_array<double>(new double[variables+1]);
SimpleLCP(variables, num_non_zeroes, m_i.get(), m_j.get(), m_ij.get(), dq.get(), lb.get(), ub.get(), &termination, x_end.get(), tol);
// We now copy x_end into z
z.resize(variables);
for (i = 0;i < variables; i++)
z[i] = x_end[i];
return (termination == MCP_Solved);
}