void CRealTextParser::PopTag(std::list<Tag>& p_rlistTags, const std::wstring& p_crszTagName)
{
for (auto riter = p_rlistTags.crbegin(); riter != p_rlistTags.crend(); ++riter) {
if (riter->m_szName == p_crszTagName) {
p_rlistTags.erase((++riter).base());
return;
}
}
}
static std::list<Eigen::Matrix<double,4,12> > lqr(System const & sys, double dt, std::list<State> const & list_state, std::list<U> const & list_u, Mat12 Q, Mat4 Ru, Mat12 Qf)
{
std::list<Eigen::Matrix<double,4,12> > list_K;
std::list<State>::const_reverse_iterator rit_state=list_state.crbegin();
std::list<U>::const_reverse_iterator rit_u=list_u.crbegin();
rit_state++;
Mat4 MF=(Mat4()<< 0, sys.kt*sys.d, 0, -sys.kt*sys.d,
-sys.kt*sys.d, 0, sys.kt*sys.d, 0,
sys.km, -sys.km, sys.km, -sys.km,
sys.kt, sys.kt, sys.kt, sys.kt).finished();
Mat12 P=Qf;
while(rit_state!=list_state.crend())
{
Vec4 u1=*rit_u;
Vec4 u2=u1.cwiseProduct(u1);
Mat3 R=rit_state->g.block(0,0,3,3);
Vec3 omega=rit_state->v.head(3);
Mat3 Ad=SO3::exp(-omega*dt);
double f=(u2(0)+u2(1)+u2(2)+u2(3))*sys.kt;
Mat12 A=Mat12::Zero();
A.block(0,6,6,6)=Mat6::Identity();
A.block(6,6,3,3)=sys.I_inv*(SO3::hat(sys.I*omega)-SO3::hat(omega)*sys.I);
A.block(9,0,3,3)=-f*R*SO3::E[2]/sys.m;
A.block(0,6,3,6)=SO3::dexp(omega*dt)*A.block(0,6,3,6);
A=Mat12::Identity()+A*dt;
A.block(0,0,3,12)=SO3::exp(-omega*dt)*A.block(0,0,3,12);
Eigen::Matrix<double,12,4> B=Eigen::Matrix<double,12,4>::Zero();
B.block(6,0,3,3)=sys.I_inv;
B.block(9,3,3,1)=R.col(2)/sys.m;
B=B*MF*(2*u1).asDiagonal();
B=B*dt;
Eigen::Matrix<double,4,12> K=(Ru+B.transpose()*P*B).inverse()*B.transpose()*P*A;
P=K.transpose()*Ru*K+Q+(A-B*K).transpose()*P*(A-B*K);
list_K.push_front(K);
rit_state++;
rit_u++;
}
return list_K;
}
void ofxWidget::updateVisibleWidgetsList() {
if (!ofxWidget::bVisibleListDirty)
return;
//ofLogNotice() << "vList Update cache miss";
// build a list of visible widgets based on all widgets
// TODO: only rebuild this list if visiblility list on adding/removing and
// parenting widgets, and on changing widget visiblity, of course.
sVisibleWidgets.clear();
for (auto it = sAllWidgets.crbegin(); it != sAllWidgets.crend(); ++it) {
if (auto p = it->lock()) {
if (p->mVisible) {
sVisibleWidgets.emplace_front(*it);
} else {
std::advance(it, p->mNumChildren);
}
}
}
ofxWidget::bVisibleListDirty = false;
}