示例#1
0
/// 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;
};
示例#3
0
/// 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;
}
示例#4
0
/// 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;
}
示例#5
0
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);
}