virtual bool initialize() {
const BodyPtr& io = ioBody();
qinit.resize(DOF);
for(int i=0; i<DOF; i++){
io->joint(i)->u() = 0.0;
qinit[i] = io->joint(i)->q();
}
qref.resize(DOF);
dqref.resize(DOF);
return true;
}
void FunctionApproximator::getParameterVectorSelectedMinMax(VectorXd& min, VectorXd& max) const
{
if (model_parameters_==NULL)
{
cerr << __FILE__ << ":" << __LINE__ << ": Warning: Trying to access model parameters of the function approximator, but it has not been trained yet. Returning empty parameter vector." << endl;
min.resize(0);
max.resize(0);
return;
}
model_parameters_->getParameterVectorSelectedMinMax(min,max);
}
void generateBezierCurve()
{
ts = VectorXd::LinSpaced ( 21,0,1. );
t_x.resize ( ts.size() );
t_y.resize ( ts.size() );
t_z.resize ( ts.size() );
for ( int idx=0; idx< ts.size(); ++idx )
{
t_x ( idx ) = getValue ( ts ( idx ), pt_x );
t_y ( idx ) = getValue ( ts ( idx ), pt_y );
t_z ( idx ) = getValue ( ts ( idx ), pt_z );
}
}
void parameters::Likelihood(const VectorXd & eff){
VectorXd loc;
loc.resize(m_proba.rows());
loc=(m_proba.rowwise().sum().array().log());
m_loglikelihood=eff.transpose()*loc;
m_bic=m_loglikelihood - 0.5*m_nbparam*log(eff.sum());
}
int
main() {
vector<Triplet<double>> trips(4);
trips.push_back(Triplet<double>(0, 0, .435));
trips.push_back(Triplet<double>(1, 1, .435));
trips.push_back(Triplet<double>(2, 2, .435));
trips.push_back(Triplet<double>(3, 3, .435));
SparseMatrix<double> A;
A.resize(4, 4);
A.setFromTriplets(trips.begin(), trips.end());
SparseQR<SparseMatrix<double>, COLAMDOrdering<int>> solverA;
solverA.compute(A);
VectorXd B;
B.resize(4);
B(0) = .435;
B(1) = .435;
B(2) = .435;
B(3) = .435;
VectorXd X = solverA.solve(B);
for (int i = 0; i < 4; i++) cout << X(i) << endl;
}
void pca_small(MatrixXd &B, int method, MatrixXd& U, VectorXd &d, bool verbose)
{
if(method == METHOD_SVD)
{
verbose && std::cout << timestamp() << " SVD begin" << std::endl;
JacobiSVD<MatrixXd> svd(B, ComputeThinU | ComputeThinV);
U = svd.matrixU();
MatrixXd V = svd.matrixV();
d = svd.singularValues().array().pow(2);
verbose && std::cout << timestamp() << " SVD done" << std::endl;
}
else if(method == METHOD_EIGEN)
{
verbose && std::cout << timestamp() << " Eigen-decomposition begin" << std::endl;
MatrixXd BBT = B * B.transpose();
verbose && std::cout << timestamp() << " dim(BBT): " << dim(BBT) << std::endl;
SelfAdjointEigenSolver<MatrixXd> eig(BBT);
// The eigenvalues come out sorted in *increasing* order,
// but we need decreasing order
VectorXd eval = eig.eigenvalues();
MatrixXd evec = eig.eigenvectors();
d.resize(eval.size());
U.resize(BBT.rows(), BBT.rows());
unsigned int k = 0, s = d.size();
for(unsigned int i = d.size() - 1 ; i != -1 ; --i)
{
// we get eigenvalues, which are the squared singular values
d(k) = eval(i);
U.col(k) = evec.col(i);
k++;
}
}
}
int contactConstraintsBV(
const RigidBodyTree<double>& r,
const KinematicsCache<double>& cache, int nc,
std::vector<double> support_mus,
// TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
drake::eigen_aligned_std_vector<SupportStateElement>& supp, MatrixXd& B,
// TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
MatrixXd& JB, MatrixXd& Jp, VectorXd& Jpdotv, MatrixXd& normals) {
int j, k = 0, nq = r.get_num_positions();
B.resize(3, nc * 2 * m_surface_tangents);
JB.resize(nq, nc * 2 * m_surface_tangents);
Jp.resize(3 * nc, nq);
Jpdotv.resize(3 * nc);
normals.resize(3, nc);
Vector3d contact_pos, pos, normal;
MatrixXd J(3, nq);
Matrix<double, 3, m_surface_tangents> d;
for (auto iter = supp.begin(); iter != supp.end(); iter++) {
double mu = support_mus[iter - supp.begin()];
double norm = sqrt(1 + mu * mu); // because normals and ds are orthogonal,
// the norm has a simple form
if (nc > 0) {
for (auto pt_iter = iter->contact_pts.begin();
pt_iter != iter->contact_pts.end(); pt_iter++) {
contact_pos = r.transformPoints(cache, *pt_iter, iter->body_idx, 0);
J = r.transformPointsJacobian(cache, *pt_iter, iter->body_idx, 0, true);
normal = iter->support_surface.head(3);
surfaceTangents(normal, d);
for (j = 0; j < m_surface_tangents; j++) {
B.col(2 * k * m_surface_tangents + j) =
(normal + mu * d.col(j)) / norm;
B.col((2 * k + 1) * m_surface_tangents + j) =
(normal - mu * d.col(j)) / norm;
JB.col(2 * k * m_surface_tangents + j) =
J.transpose() * B.col(2 * k * m_surface_tangents + j);
JB.col((2 * k + 1) * m_surface_tangents + j) =
J.transpose() * B.col((2 * k + 1) * m_surface_tangents + j);
}
// store away kin sols into Jp and Jpdotv
// NOTE: I'm cheating and using a slightly different ordering of J and
// Jdot here
Jp.block(3 * k, 0, 3, nq) = J;
Vector3d pt = (*pt_iter).head(3);
Jpdotv.block(3 * k, 0, 3, 1) =
r.transformPointsJacobianDotTimesV(cache, pt, iter->body_idx, 0);
normals.col(k) = normal;
k++;
}
}
}
return k;
}
/**
* reconstruct the displacements u in Euler space with RS coordinates y
* provided.
*
* @param y the RS coordinates.
* @param u the constructed Euler coordinates represents the displacements.
*
* @return true if construction is success.
*/
bool RS2Euler::reconstruct(const VectorXd &y, VectorXd &u){
assert (tetmesh != NULL);
const int node_numx3 = tetmesh->nodes().size()*3;
bool succ = true;
// assemble the right_side
VectorXd b;
assemble_b(y,b);
assert_eq(VG_t.cols(),b.size());
assert_eq(VG_t.rows(),node_numx3);
VectorXd right_side(node_numx3 + numFixedDofs());
right_side.segment(0, node_numx3) = VG_t*b;
right_side.segment(node_numx3,numFixedDofs()).setZero();
right_side.segment(node_numx3,barycenter_uc.size()) = barycenter_uc;
// solve A*u = right_side
assert_eq (right_side.size(), A_solver.rows());
u = A_solver.solve(right_side);
// get the result
if(A_solver.info()!=Eigen::Success) {
succ = false;
u.resize(node_numx3);
u.setZero();
ERROR_LOG("failed to solve for A X = P.");
}else{
assert_gt(u.size(), node_numx3);
const VectorXd x = u.head(node_numx3);
u = x;
}
return succ;
}
void FunctionApproximator::getParameterVectorSelected(VectorXd& values, bool normalized) const
{
if (checkModelParametersInitialized())
model_parameters_->getParameterVectorSelected(values, normalized);
else
values.resize(0);
}
void ModelParametersLWPR::getParameterVectorAll(VectorXd& values) const
{
values.resize(getParameterVectorAllSize());
int ii=0;
for (int iDim = 0; iDim < lwpr_object_->model.nOut; iDim++)
for (int iRF = 0; iRF < lwpr_object_->model.sub[iDim].numRFS; iRF++)
for (int j = 0; j < lwpr_object_->nIn(); j++)
values[ii++] = lwpr_object_->model.sub[iDim].rf[iRF]->c[j];
for (int iDim = 0; iDim < lwpr_object_->model.nOut; iDim++)
for (int iRF = 0; iRF < lwpr_object_->model.sub[iDim].numRFS; iRF++)
for (int j = 0; j < lwpr_object_->nIn(); j++)
values[ii++] = lwpr_object_->model.sub[iDim].rf[iRF]->D[j*lwpr_object_->model.nInStore+j];
for (int iDim = 0; iDim < lwpr_object_->model.nOut; iDim++)
for (int iRF = 0; iRF < lwpr_object_->model.sub[iDim].numRFS; iRF++)
for (int j = 0; j < lwpr_object_->model.sub[iDim].rf[iRF]->nReg; j++)
values[ii++] = lwpr_object_->model.sub[iDim].rf[iRF]->beta[j];
for (int iDim = 0; iDim < lwpr_object_->model.nOut; iDim++)
for (int iRF = 0; iRF < lwpr_object_->model.sub[iDim].numRFS; iRF++)
values[ii++] = lwpr_object_->model.sub[iDim].rf[iRF]->beta0;
assert(ii == getParameterVectorAllSize());
};
void Reservoir::linear_activation(VectorXd &inputs, VectorXd &results) {
unsigned int num_elems = (inputs.rows() > inputs.cols()) ? inputs.rows() : inputs.cols();
results.resize(num_elems, 1);
for (unsigned int i=0; i<num_elems; i++) {
results(i) = inputs(i);
}
}
int contactPhi(RigidBodyManipulator* r, SupportStateElement& supp, void *map_ptr, VectorXd &phi, double terrain_height)
{
std::unique_ptr<RigidBody>& b = r->bodies[supp.body_idx];
int nc = supp.contact_pt_inds.size();
phi.resize(nc);
if (nc<1) return nc;
Vector3d contact_pos,pos,posB,normal; Vector4d tmp;
int i=0;
for (std::set<int>::iterator pt_iter=supp.contact_pt_inds.begin(); pt_iter!=supp.contact_pt_inds.end(); pt_iter++) {
if (*pt_iter<0 || *pt_iter>=b->contact_pts.cols())
mexErrMsgIdAndTxt("DRC:ControlUtil:BadInput","requesting contact pt %d but body only has %d pts",*pt_iter,b->contact_pts.cols());
tmp = b->contact_pts.col(*pt_iter);
r->forwardKin(supp.body_idx,tmp,0,contact_pos);
collisionDetect(map_ptr,contact_pos,pos,NULL,terrain_height);
pos -= contact_pos; // now -rel_pos in matlab version
phi(i) = pos.norm();
if (pos.dot(normal)>0)
phi(i)=-phi(i);
i++;
}
return nc;
}
VectorXd linear::field_to_vctr(const kind::f scalarf ) {
std::vector<int> indices;
std::vector<FT> ff;
for(F_v_it fv=T.finite_vertices_begin();
fv!=T.finite_vertices_end();
fv++) {
indices.push_back( fv->idx.val() );
ff.push_back( fv->sf(scalarf).val() );
}
int N=indices.size();
VectorXd vctr;
vctr.resize(N);
for(int nn=0; nn<indices.size(); nn++) {
vctr(indices[nn])=ff[nn];
// cout << indices[nn] << " : "
// << ff[nn] << endl;
}
return vctr;
}
bool BulletModel::collisionRaycast(const Matrix3Xd &origins, const Matrix3Xd &ray_endpoints, bool use_margins, VectorXd &distances)
{
distances.resize(origins.cols());
BulletCollisionWorldWrapper& bt_world = getBulletWorld(use_margins);
for (int i = 0; i < origins.cols(); i ++)
{
btVector3 ray_from_world(origins(0,i), origins(1,i), origins(2,i));
btVector3 ray_to_world(ray_endpoints(0,i), ray_endpoints(1,i), ray_endpoints(2,i));
btCollisionWorld::ClosestRayResultCallback ray_callback(ray_from_world, ray_to_world);
bt_world.bt_collision_world->rayTest(ray_from_world, ray_to_world, ray_callback);
if (ray_callback.hasHit()) {
// compute distance to hit
btVector3 end = ray_callback.m_hitPointWorld;
Vector3d end_eigen(end.getX(), end.getY(), end.getZ());
distances(i) = (end_eigen - origins.col(i)).norm();
} else {
distances(i) = -1;
}
}
return true;
}
bool EEGoSportsDriver::getSampleMatrixValue(Eigen::MatrixXd &sampleMatrix)
{
//Check if device was initialised and connected correctly
if(!m_bInitDeviceSuccess) {
std::cout << "Plugin EEGoSports - ERROR - getSampleMatrixValue() - Cannot start to get samples from device because device was not initialised correctly" << std::endl;
return false;
}
sampleMatrix = MatrixXd::Zero(m_uiNumberOfChannels, m_uiSamplesPerBlock);
uint iSampleIterator, iReceivedSamples, iChannelCount, i, j;
iSampleIterator = 0;
buffer buf;
VectorXd vec;
//get samples from device until the complete matrix is filled, i.e. the samples per block size is met
while(iSampleIterator < m_uiSamplesPerBlock) {
//Get sample block from device
buf = m_pDataStream->getData();
iReceivedSamples = buf.getSampleCount();
iChannelCount = buf.getChannelCount();
//Write the received samples to an extra buffer, so that they are not getting lost if too many samples were received. These are then written to the next matrix (block)
for(i = 0; i < iReceivedSamples; ++i) {
vec.resize(iChannelCount);
for(j = 0; j < iChannelCount; ++j) {
vec(j) = buf.getSample(j,i);
//std::cout<<vec(j)<<std::endl;
}
m_lSampleBlockBuffer.push_back(vec);
}
//Fill matrix with data from buffer
while(!m_lSampleBlockBuffer.isEmpty()) {
if(iSampleIterator >= m_uiSamplesPerBlock) {
break;
}
sampleMatrix.col(iSampleIterator) = m_lSampleBlockBuffer.takeFirst();
iSampleIterator++;
}
if(m_outputFileStream.is_open() && m_bWriteDriverDebugToFile) {
m_outputFileStream << "buf.getSampleCount(): " << buf.getSampleCount() << std::endl;
m_outputFileStream << "buf.getChannelCount(): " << buf.getChannelCount() << std::endl;
m_outputFileStream << "buf.size(): " << buf.size() << std::endl;
m_outputFileStream << "iSampleIterator: " << iSampleIterator << std::endl;
m_outputFileStream << "m_lSampleBlockBuffer.size(): " << m_lSampleBlockBuffer.size() << std::endl << std::endl;
}
}
return true;
}
/** Cost function
* \param[in] cost_vars y in \f$ y = a*x^2 + c \f$
* \param[in] sample The sample from which cost_vars was generated. Required for regularization.
* \param[in] task_parameters Ignored
* \param[out] cost Cost of the rollout.
*/
void evaluateRollout(const MatrixXd& cost_vars, const VectorXd& sample, const VectorXd& task_parameters, VectorXd& cost) const
{
VectorXd diff_square = (cost_vars.array()-targets_.array()).square();
cost.resize(3);
cost[1] = diff_square.mean();
cost[2] = regularization_weight_*sqrt(sample.array().pow(2).sum());
cost[0] = cost[1] + cost[2];
}
/** The cost function which defines the cost_function.
*
* \param[in] sample The sample
* \param[out] cost The scalar cost for each sample.
*/
void evaluate(const VectorXd& sample, VectorXd& cost) const
{
assert(sample.size()==point_.size());
// Cost is distance to point
double dist_to_point = sqrt((sample - point_).array().pow(2).sum());
if (regularization_weight_>0.0)
{
cost.resize(3);
cost[1] = dist_to_point;
cost[2] = regularization_weight_*sqrt(sample.array().pow(2).sum());
cost[0] = cost[1] + cost[2];
}
else
{
cost.resize(1);
cost[0] = dist_to_point;
}
}
virtual bool initialize() {
if(!qseq){
string filename = getNativePathString(
boost::filesystem::path(shareDirectory())
/ "motion" / "SR1" / "SR1WalkPattern3.yaml");
BodyMotion motion;
if(!motion.loadStandardYAMLformat(filename)){
os() << motion.seqMessage() << endl;
return false;
}
qseq = motion.jointPosSeq();
if(qseq->numFrames() == 0){
os() << "Empty motion data." << endl;
return false;
}
timeStep_ = qseq->getTimeStep();
}
if(fabs(timeStep() - timeStep_) > 1.0e-6){
os() << "Time step must be " << timeStep_ << "." << endl;;
return false;
}
body = ioBody();
numJoints = body->numJoints();
if(numJoints != qseq->numParts()){
os() << "The number of joints must be " << qseq->numParts() << endl;
return false;
}
qold.resize(numJoints);
VectorXd q0(numJoints);
VectorXd q1(numJoints);
for(int i=0; i < numJoints; ++i){
qold[i] = q0[i] = body->joint(i)->q();
}
MultiValueSeq::Frame frame = qseq->frame(0);
for(int i=0; i < numJoints; ++i){
q1[i] = frame[i];
}
interpolator.clear();
interpolator.appendSample(0.0, q0);
interpolator.appendSample(2.0, q1);
interpolator.update();
qref_old = interpolator.interpolate(0.0);
currentFrame = 0;
phase = 0;
time = 0.0;
return true;
}
void EEMS::calc_within(const VectorXi &qColors, const VectorXd &qEffcts, VectorXd &W) const {
// o is the number of observed demes in the graph
if (W.size()!=o) { W.resize(o); }
// For every observed deme in the graph
for ( int alpha = 0 ; alpha < o ; alpha++ ) {
// WithinDiversity = W(a) = 10^( q_alpha ) = 10^( q_tile(tile_alpha))
W(alpha) = pow(10.0,qEffcts(qColors(alpha)));
}
// The constant is slightly different for haploid and diploid species
W *= Wconst;
}
JointPathIkImpl() {
deltaScale = JointPath::numericalIKdefaultDeltaScale();
maxIterations = JointPath::numericalIKdefaultMaxIterations();
iteration = 0;
dTask.resize(6);
isBestEffortIKmode = false;
double e = JointPath::numericalIKdefaultMaxIKerror();
maxIKerrorSqr = e * e;
double d = JointPath::numericalIKdefaultDampingConstant();
dampingConstantSqr = d * d;
}
// Method to compute location of given a vector of joint configurations; note the
// returned vector could be 3D (X,Y,Z) or 6D(X,Y,Z,R,P,Y)
VectorXd JointMover::GetXYZRPY( VectorXd _q, bool both ) {
// Get current XYZ position
mRobot->setConfig(mLinks, _q);
MatrixXd qTransform = mEENode->getWorldTransform();
VectorXd qXYZRPY;
//return a 6D vector if both x,y,z and r,p,y must be computed
if(both){
Matrix3d rotM = qTransform.topLeftCorner(3,3);
VectorXd rot = rotM.eulerAngles(0,1,2);
qXYZRPY.resize(6);
qXYZRPY << qTransform(0,3), qTransform(1,3), qTransform(2,3), rot(0), rot(1), rot(2);
}
else{
qXYZRPY.resize(3);
qXYZRPY << qTransform(0,3), qTransform(1,3), qTransform(2,3);
}
return qXYZRPY;
}
void Dmp::getParameterVectorAll(VectorXd& values) const
{
values.resize(getParameterVectorAllSize());
int offset = 0;
VectorXd cur_values;
for (int dd=0; dd<dim_orig(); dd++)
{
function_approximators_[dd]->getParameterVectorAll(cur_values);
values.segment(offset,cur_values.size()) = cur_values;
offset += cur_values.size();
}
}
int contactConstraintsBV(RigidBodyManipulator *r, int nc, std::vector<double> support_mus, std::vector<SupportStateElement,Eigen::aligned_allocator<SupportStateElement>>& supp, void *map_ptr, MatrixXd &B, MatrixXd &JB, MatrixXd &Jp, VectorXd &Jpdotv, MatrixXd &normals, double terrain_height)
{
int j, k=0, nq = r->num_positions;
B.resize(3,nc*2*m_surface_tangents);
JB.resize(nq,nc*2*m_surface_tangents);
Jp.resize(3*nc,nq);
Jpdotv.resize(3*nc);
normals.resize(3, nc);
Vector3d contact_pos,pos,normal;
MatrixXd J(3,nq);
Matrix<double,3,m_surface_tangents> d;
for (std::vector<SupportStateElement,Eigen::aligned_allocator<SupportStateElement>>::iterator iter = supp.begin(); iter!=supp.end(); iter++) {
double mu = support_mus[iter - supp.begin()];
double norm = sqrt(1+mu*mu); // because normals and ds are orthogonal, the norm has a simple form
if (nc>0) {
for (auto pt_iter=iter->contact_pts.begin(); pt_iter!=iter->contact_pts.end(); pt_iter++) {
auto contact_pos_gradientvar = r->forwardKin(*pt_iter, iter->body_idx, 0, 0, 1);
contact_pos = contact_pos_gradientvar.value();
J = contact_pos_gradientvar.gradient().value();
if (iter->use_support_surface) {
normal = iter->support_surface.head(3);
}
else {
collisionDetect(map_ptr,contact_pos,pos,&normal,terrain_height);
}
surfaceTangents(normal,d);
for (j=0; j<m_surface_tangents; j++) {
B.col(2*k*m_surface_tangents+j) = (normal + mu*d.col(j)) / norm;
B.col((2*k+1)*m_surface_tangents+j) = (normal - mu*d.col(j)) / norm;
JB.col(2 * k * m_surface_tangents + j) = J.transpose() * B.col(2 * k * m_surface_tangents + j);
JB.col((2 * k + 1) * m_surface_tangents + j) = J.transpose() * B.col((2*k+1)*m_surface_tangents+j);
}
// store away kin sols into Jp and Jpdotv
// NOTE: I'm cheating and using a slightly different ordering of J and Jdot here
Jp.block(3*k,0,3,nq) = J;
Vector3d pt = (*pt_iter).head(3);
auto Jpdotv_grad = r->forwardJacDotTimesV(pt,iter->body_idx,0,0,0);
Jpdotv.block(3*k,0,3,1) = Jpdotv_grad.value();
normals.col(k) = normal;
k++;
}
}
}
return k;
}
void RS2Euler::assemble_b(const VectorXd &y,VectorXd & b)const{
const int elem_num = tetmesh->tets().size();
assert_eq(y.size(), elem_num*9);
b.resize(elem_num*9);
/// compute b from y
for (int j = 0; j < elem_num; ++j){
const double *yj = &(y[9*j]);
double *bj = &(b[9*j]);
ComputeBj::compute(yj,Sqrt_V[j],bj);
}
}
/** Cost function
* \param[in] cost_vars y in \f$ y = a*x^2 + c \f$
* \param[in] task_parameters Ignored
* \param[out] costs Costs of the cost_vars
*/
void evaluate(const MatrixXd& cost_vars, const MatrixXd& task_parameters, VectorXd& costs) const
{
int n_samples = cost_vars.rows();
costs.resize(n_samples);
VectorXd predictions, diff_square;
for (int k=0; k<n_samples; k++)
{
predictions = cost_vars.row(k);
diff_square = (predictions.array()-targets_.array()).square();
costs[k] = diff_square.mean();
}
}
const VectorXd &AniEditDM::getUforConstraint(const int frame)const{
static VectorXd tempt_u0;
if (interpolator != NULL && totalFrameNum() > 0){
assert_in(frame,0, totalFrameNum()-1);
return interpolator->getUforConstraint(frame);
}else{
if (tempt_u0.size() != fullDim()){
tempt_u0.resize(fullDim());
tempt_u0.setZero();
}
return tempt_u0;
}
}
int contactPhi(RigidBodyTree * r, const KinematicsCache<double>& cache, SupportStateElement& supp, VectorXd &phi)
{
int nc = static_cast<int>(supp.contact_pts.size());
phi.resize(nc);
if (nc<1) return nc;
int i=0;
for (auto pt_iter = supp.contact_pts.begin(); pt_iter != supp.contact_pts.end(); pt_iter++) {
Vector3d contact_pos = r->transformPoints(cache, *pt_iter, supp.body_idx, 0);
phi(i) = supp.support_surface.head<3>().dot(contact_pos) + supp.support_surface(3);
i++;
}
return nc;
}
/** The cost function which defines the cost_function.
*
* \param[in] samples The samples
* \param[out] costs The scalar cost for each sample.
*/
void evaluate(const MatrixXd& samples, VectorXd& costs) const {
#ifndef NDEBUG // Variables below are only required for asserts; check for NDEBUG to avoid warnings.
int n_dims = samples.cols();
#endif
assert(n_dims==point_.size());
int n_samples = samples.rows();
costs.resize(n_samples);
for (int ss=0; ss<n_samples; ss++)
{
// Cost is distance to point
costs[ss] = sqrt((samples.row(ss) - point_.transpose()).array().pow(2).sum());
}
}
void Iterative_Control::thread_to_state(const Thread* thread, VectorXd& state)
{
const int num_pieces = thread->num_pieces();
state.resize(_size_each_state);
for (int piece_ind=0; piece_ind < thread->num_pieces(); piece_ind++)
{
state.segment(piece_ind*3, 3) = thread->vertex_at_ind(piece_ind);
if (piece_ind < thread->num_edges()) {
state.segment(piece_ind*3 + 3*num_pieces, 3) = thread->edge_at_ind(piece_ind);
}
}
state(6*num_pieces - 3) = thread->end_angle();
state(6*num_pieces - 2) = ((Thread*)thread)->calculate_energy();
//state(3*num_pieces) = thread->end_angle();
//state(3*num_pieces+1) = ((Thread*)thread)->calculate_energy();
}
void loadMesh(const char *filename, VectorXd &verts, Matrix3Xi &faces)
{
ifstream ifs(filename);
if(!ifs)
return;
vector<double> pos;
vector<int> faceidx;
while(true)
{
char c;
ifs >> c;
if(!ifs)
break;
if(c == 'v')
{
for(int i=0; i<3; i++)
{
double p;
ifs >> p;
pos.push_back(p);
}
}
if(c == 'f')
{
for(int i=0; i<3; i++)
{
int vert;
ifs >> vert;
faceidx.push_back(vert);
}
}
}
int nverts = pos.size()/3;
int nfaces = faceidx.size()/3;
verts.resize(3*nverts);
for(int i=0; i<3*nverts; i++)
verts[i] = pos[i];
faces.resize(3, nfaces);
for(int i=0; i<nfaces; i++)
for(int j=0; j<3; j++)
faces.coeffRef(j, i) = faceidx[3*i+j]-1;
}