本文整理汇总了C++中VectorNd::resize方法的典型用法代码示例。如果您正苦于以下问题:C++ VectorNd::resize方法的具体用法?C++ VectorNd::resize怎么用?C++ VectorNd::resize使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类VectorNd
的用法示例。
在下文中一共展示了VectorNd::resize方法的4个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: calc_gradient
// Computes the gradient for conservative advancement
// gradient: | (radius * -sin theta * sin phi) * y(1) + ...
// (radius * cos theta * sin phi) * y(2) |
// | (radius * cos theta * cos phi) * y(1) + ...
// (radius * sin theta * cos phi) * y(2) + ...
// (radius * -sin phi) * y(3) |
void SpherePrimitive::calc_gradient(const VectorNd& x, void* data, VectorNd& g)
{
// get the pair
std::pair<double, VectorNd>& data_pair = *((std::pair<double, VectorNd>*) data);
// get radius and y
double r = data_pair.first;
VectorNd& y = data_pair.second;
// get components of y
const double Y1 = y[0];
const double Y2 = y[1];
const double Y3 = y[2];
// get theta and phi
double THETA = x[0];
double PHI = x[1];
// compute trig functions
double CTHETA = std::cos(THETA);
double STHETA = std::sin(THETA);
double CPHI = std::cos(PHI);
double SPHI = std::sin(PHI);
// setup gradient
g.resize(2);
g[0] = (-STHETA * SPHI) * Y1 + (CTHETA * SPHI) * Y2;
g[1] = (CTHETA * CPHI) * Y1 + (STHETA * CPHI) * Y2 - SPHI * Y3;
g *= -r;
}
示例2: determine_q
/// Determines (and sets) the value of Q from the axes and the inboard link and outboard link transforms
void UniversalJoint::determine_q(VectorNd& q)
{
const unsigned X = 0, Y = 1, Z = 2;
// get the outboard link
RigidBodyPtr outboard = get_outboard_link();
// verify that the outboard link is set
if (!outboard)
throw std::runtime_error("determine_q() called on NULL outboard link!");
// set proper size for q
this->q.resize(num_dof());
// get the poses of the joint and outboard link
shared_ptr<const Pose3d> Fj = get_pose();
shared_ptr<const Pose3d> Fo = outboard->get_pose();
// compute transforms
Transform3d wTo = Pose3d::calc_relative_pose(Fo, GLOBAL);
Transform3d jTw = Pose3d::calc_relative_pose(GLOBAL, Fj);
Transform3d jTo = jTw * wTo;
// determine the joint transformation
Matrix3d R = jTo.q;
// determine q1 and q2 -- they are uniquely determined by examining the rotation matrix
// (see get_rotation())
q.resize(num_dof());
q[DOF_1] = std::atan2(R(Z,Y), R(Y,Y));
q[DOF_2] = std::atan2(R(X,Z), R(X,X));
}
示例3: compute_signed_dist_dot_Jacobians
/**
* Assume the signed distance function is Phi(q(t)), so
* Phi(q(t_0 + \Delta t))_{t_0} \approx Phi(q(t_0)) + d/dt Phi(q(t_0)) * dt ==>
* d/dt Phi(q(t_0)) \approx (Phi(q(t_0 + \Delta t)) - Phi(q(t_0))/dt
*/
void SignedDistDot::compute_signed_dist_dot_Jacobians(UnilateralConstraintProblemData& q, MatrixNd& Cdot_iM_CnT, MatrixNd& Cdot_iM_CsT, MatrixNd& Cdot_iM_CtT, MatrixNd& Cdot_iM_LT, VectorNd& Cdot_v)
{
const double DT = NEAR_ZERO;
vector<shared_ptr<DynamicBodyd> > tmp_supers1, tmp_supers2, isect;
VectorNd gc, gv;
// get all pairs of bodies involved in contact
vector<shared_ptr<DynamicBodyd> > ubodies;
for (unsigned i=0; i< q.signed_distances.size(); i++)
{
// get the two single bodies
shared_ptr<SingleBodyd> s1 = q.signed_distances[i].a->get_single_body();
shared_ptr<SingleBodyd> s2 = q.signed_distances[i].b->get_single_body();
// get the two super bodies
shared_ptr<DynamicBodyd> sb1 = ImpactConstraintHandler::get_super_body(s1);
shared_ptr<DynamicBodyd> sb2 = ImpactConstraintHandler::get_super_body(s2);
// add the bodies to ubodies
ubodies.push_back(sb1);
ubodies.push_back(sb2);
}
// get all unique bodies involved in contact
std::sort(ubodies.begin(), ubodies.end());
ubodies.erase(std::unique(ubodies.begin(), ubodies.end()), ubodies.end());
// save all configurations for all bodies involved in contact
map<shared_ptr<DynamicBodyd>, VectorNd> gc_map;
for (unsigned i=0; i< ubodies.size(); i++)
ubodies[i]->get_generalized_coordinates_euler(gc_map[ubodies[i]]);
// save all velocities for all bodies involved in contact
map<shared_ptr<DynamicBodyd>, VectorNd> gv_map;
for (unsigned i=0; i< ubodies.size(); i++)
ubodies[i]->get_generalized_velocity(DynamicBodyd::eSpatial, gv_map[ubodies[i]]);
// resize Cdot(v)
Cdot_v.resize(q.signed_distances.size());
// for each pair of bodies
for (unsigned k=0; k< q.signed_distances.size(); k++)
{
// get the two single bodies
shared_ptr<SingleBodyd> s1 = q.signed_distances[k].a->get_single_body();
shared_ptr<SingleBodyd> s2 = q.signed_distances[k].b->get_single_body();
// get the signed distance between the two bodies
double phi = q.signed_distances[k].dist;
// integrates bodies' positions forward
shared_ptr<DynamicBodyd> sup1 = ImpactConstraintHandler::get_super_body(s1);
shared_ptr<DynamicBodyd> sup2 = ImpactConstraintHandler::get_super_body(s2);
tmp_supers1.clear();
tmp_supers1.push_back(sup1);
if (sup1 != sup2)
tmp_supers1.push_back(sup2);
integrate_positions(tmp_supers1, DT);
// compute the signed distance function
double phi_new = calc_signed_dist(s1, s2);
// set the appropriate entry of Cdot(v)
Cdot_v[k] = (phi_new - phi)/DT;
// restore coordinates and velocities
restore_coords_and_velocities(tmp_supers1, gc_map, gv_map);
}
// resize the Jacobians
Cdot_iM_CnT.resize(q.signed_distances.size(), q.N_CONTACTS);
Cdot_iM_CsT.resize(q.signed_distances.size(), q.N_CONTACTS);
Cdot_iM_CtT.resize(q.signed_distances.size(), q.N_CONTACTS);
Cdot_iM_LT.resize(q.signed_distances.size(), q.N_LIMITS);
// prepare iterators for contacts
ColumnIteratord Cn_iter = Cdot_iM_CnT.column_iterator_begin();
ColumnIteratord Cs_iter = Cdot_iM_CsT.column_iterator_begin();
ColumnIteratord Ct_iter = Cdot_iM_CtT.column_iterator_begin();
ColumnIteratord L_iter = Cdot_iM_LT.column_iterator_begin();
// for each pair of bodies
for (unsigned k=0; k< q.signed_distances.size(); k++)
{
// get the two single bodies
shared_ptr<SingleBodyd> s1 = q.signed_distances[k].a->get_single_body();
shared_ptr<SingleBodyd> s2 = q.signed_distances[k].b->get_single_body();
// get the two bodies involved
shared_ptr<DynamicBodyd> sup1 = ImpactConstraintHandler::get_super_body(s1);
shared_ptr<DynamicBodyd> sup2 = ImpactConstraintHandler::get_super_body(s2);
tmp_supers1.clear();
tmp_supers1.push_back(sup1);
tmp_supers1.push_back(sup2);
//.........这里部分代码省略.........
示例4: solve_nqp_work
/**
* \param x the solution is returned here; zeros will be returned at appropriate indices for inactive contacts
*/
void ImpactConstraintHandler::solve_nqp_work(UnilateralConstraintProblemData& q, VectorNd& x)
{
const double INF = std::numeric_limits<double>::max();
// setup constants
const unsigned N_CONTACTS = q.N_CONTACTS;
const unsigned N_LIMITS = q.N_LIMITS;
const unsigned N_CONSTRAINT_EQNS_IMP = q.N_CONSTRAINT_EQNS_IMP;
const unsigned CN_IDX = 0;
const unsigned CS_IDX = N_CONTACTS;
const unsigned CT_IDX = CS_IDX + N_CONTACTS;
const unsigned CL_IDX = CT_IDX + N_CONTACTS;
const unsigned NVARS = N_LIMITS + CL_IDX;
// setup the optimization data
_ipsolver->epd = &q;
_ipsolver->mu_c.resize(N_CONTACTS);
_ipsolver->mu_visc.resize(N_CONTACTS);
// setup true friction cone for every contact
for (unsigned i=0; i< N_CONTACTS; i++)
{
_ipsolver->mu_c[i] = sqr(q.contact_constraints[i]->contact_mu_coulomb);
_ipsolver->mu_visc[i] = (sqr(q.Cs_v[i]) + sqr(q.Ct_v[i])) *
sqr(q.contact_constraints[i]->contact_mu_viscous);
}
// setup matrices
MatrixNd& R = _ipsolver->R;
MatrixNd& H = _ipsolver->H;
VectorNd& c = _ipsolver->c;
VectorNd& z = _ipsolver->z;
// init z (particular solution)
z.set_zero(NVARS);
// first, compute the appropriate nullspace
if (N_CONSTRAINT_EQNS_IMP > 0)
{
// compute the homogeneous solution
_A = q.Jx_iM_JxT;
(_workv = q.Jx_v).negate();
try
{
_LA.solve_LS_fast1(_A, _workv);
}
catch (NumericalException e)
{
_A = q.Jx_iM_JxT;
_LA.solve_LS_fast2(_A, _workv);
}
z.set_sub_vec(q.ALPHA_X_IDX, _workv);
// setup blocks of A
_A.resize(N_CONSTRAINT_EQNS_IMP, NVARS);
SharedMatrixNd b1 = _A.block(0, N_CONSTRAINT_EQNS_IMP, 0, N_CONTACTS);
SharedMatrixNd b2 = _A.block(0, N_CONSTRAINT_EQNS_IMP, N_CONTACTS, N_CONTACTS*2);
SharedMatrixNd b3 = _A.block(0, N_CONSTRAINT_EQNS_IMP, N_CONTACTS*2, N_CONTACTS*3);
SharedMatrixNd b4 = _A.block(0, N_CONSTRAINT_EQNS_IMP, N_CONTACTS*3, N_CONTACTS*3+N_LIMITS);
// compute the nullspace
MatrixNd::transpose(q.Cn_iM_JxT, b1);
MatrixNd::transpose(q.Cs_iM_JxT, b2);
MatrixNd::transpose(q.Ct_iM_JxT, b3);
MatrixNd::transpose(q.L_iM_JxT, b4);
_LA.nullspace(_A, R);
}
else
// clear the nullspace
R.resize(0,0);
// get number of qp variables
const unsigned N_PRIMAL = (R.columns() > 0) ? R.columns() : NVARS;
// setup number of nonlinear inequality constraints
const unsigned NONLIN_INEQUAL = N_CONTACTS;
// init the QP matrix and vector
H.resize(N_PRIMAL, N_PRIMAL);
c.resize(H.rows());
// setup row (block) 1 -- Cn * iM * [Cn' Cs Ct' L']
unsigned col_start = 0, col_end = N_CONTACTS;
unsigned row_start = 0, row_end = N_CONTACTS;
SharedMatrixNd Cn_iM_CnT = H.block(row_start, row_end, col_start, col_end);
col_start = col_end; col_end += N_CONTACTS;
SharedMatrixNd Cn_iM_CsT = H.block(row_start, row_end, col_start, col_end);
col_start = col_end; col_end += N_CONTACTS;
SharedMatrixNd Cn_iM_CtT = H.block(row_start, row_end, col_start, col_end);
col_start = col_end; col_end += N_LIMITS;
SharedMatrixNd Cn_iM_LT = H.block(row_start, row_end, col_start, col_end);
// setup row (block) 2 -- Cs * iM * [Cn' Cs' Ct' L']
row_start = row_end; row_end += N_CONTACTS;
col_start = 0; col_end = N_CONTACTS;
SharedMatrixNd Cs_iM_CnT = H.block(row_start, row_end, col_start, col_end);
col_start = col_end; col_end += N_CONTACTS;
//.........这里部分代码省略.........