bool LinkGroup::load(const Body& body, const Listing& linkGroupList)
{
for(int i=0; i < linkGroupList.size(); ++i) {
const ValueNode& node = linkGroupList[i];
if(node.isScalar()) {
Link* link = body.link(node.toString());
if(!link) {
return false;
}
elements.push_back(link->index());
} else if(node.isMapping()) {
const Mapping& group = *node.toMapping();
LinkGroupPtr linkGroup = boost::make_shared<LinkGroup>(private_tag());
linkGroup->setName(group["name"]);
if(linkGroup->load(body, *group["links"].toListing())) {
elements.push_back(linkGroup);
} else {
return false;
}
} else {
return false;
}
}
return true;
}
void YAMLReaderImpl::onListingEnd(yaml_event_t& event)
{
if(debugTrace){
cout << "YAMLReaderImpl::onListingEnd()" << endl;
}
if(isRegularMultiListingExpected){
Listing* listing = static_cast<Listing*>(nodeStack.top().node.get());
const int level = nodeStack.size() - 1;
expectedListingSizes[level] = listing->size();
}
popNode(event);
}
static void readAffine3(const Listing& node, Affine3& out_value)
{
if(node.size() == 6){
Affine3::TranslationPart t = out_value.translation();
t[0] = node[0].toDouble();
t[1] = node[1].toDouble();
t[2] = node[2].toDouble();
const double r = node[3].toDouble();
const double p = node[4].toDouble();
const double y = node[5].toDouble();
out_value.linear() = rotFromRpy(r, p, y);
}
}
void replaceDirectoryVariable(ArchiveSharedData* shared, QString& io_pathString, const QString& varname, int pos, int len)
{
Listing* paths = shared->directoryVariableMap->findListing(varname.toStdString());
if(paths){
for(int i=0; i < paths->size(); ++i){
string vpath;
QString replaced(io_pathString);
replaced.replace(pos, len, paths->at(i)->toString().c_str());
filesystem::file_status fstatus = filesystem::status(filesystem::path(replaced.toStdString()));
if(filesystem::is_directory(fstatus) || filesystem::exists(fstatus)) {
io_pathString = replaced;
return;
}
}
}
MessageView::mainInstance()->putln(
MessageView::WARNING,
QString(_("${%1} of \"%2\" cannot be expanded !")).arg(varname).arg(io_pathString));
}
bool ParametricPathProcessorImpl::replaceDirectoryVariable
(QString& io_pathString, const string& varname, int pos, int len)
{
Listing* paths = variables->findListing(varname);
if(paths) {
for(int i=0; i < paths->size(); ++i) {
string vpath;
QString replaced(io_pathString);
replaced.replace(pos, len, paths->at(i)->toString().c_str());
filesystem::file_status fstatus = filesystem::status(filesystem::path(replaced.toStdString()));
if(filesystem::is_directory(fstatus) || filesystem::exists(fstatus)) {
io_pathString = replaced;
return true;
}
}
}
errorMessage = str(format(_("%1% of \"%2%\" cannot be expanded !")) % varname % io_pathString.toStdString());
return false;
}
/**
\todo Introduce a tree structure to improve the efficiency of searching matched directories
*/
bool ParametricPathProcessorImpl::findSubDirectoryOfDirectoryVariable
(const filesystem::path& path, std::string& out_varName, filesystem::path& out_relativePath)
{
out_relativePath.clear();
int maxMatchSize = 0;
filesystem::path relativePath;
Mapping::const_iterator p;
for(p = variables->begin(); p != variables->end(); ++p) {
Listing* paths = p->second->toListing();
if(paths) {
for(int i=0; i < paths->size(); ++i) {
filesystem::path dirPath(paths->at(i)->toString());
int n = findSubDirectory(dirPath, path, relativePath);
if(n > maxMatchSize) {
maxMatchSize = n;
out_relativePath = relativePath;
out_varName = fromUTF8(p->first);
}
}
}
}
return (maxMatchSize > 0);
}
bool BodyItemImpl::restore(const Archive& archive)
{
bool restored = false;
string modelFile;
/*
if(archive.readRelocatablePath("modelFile", modelFile)){
restored = modelFile.empty() || load(modelFile);
}
*/
if(archive.readRelocatablePath("modelFile", modelFile)){
restored = self->load(modelFile);
}
if(restored){
Vector3 p;
if(read(archive, "rootPosition", p)){
body->rootLink()->p() = p;
}
Matrix3 R;
if(read(archive, "rootAttitude", R)){
body->rootLink()->R() = R;
}
Listing* qs = archive.findListing("jointPositions");
if(qs->isValid()){
for(int i=0; i < qs->size(); ++i){
body->joint(i)->q() = (*qs)[i].toDouble();
}
}
//! \todo replace the following code with the ValueTree serialization function of BodyState
initialState.clear();
if(read(archive, "initialRootPosition", p) && read(archive, "initialRootAttitude", R)){
initialState.setRootLinkPosition(SE3(p, R));
}
qs = archive.findListing("initialJointPositions");
if(qs->isValid()){
BodyState::Data& q = initialState.data(BodyState::JOINT_POSITIONS);
q.resize(qs->size());
for(int i=0; i < qs->size(); ++i){
q[i] = (*qs)[i].toDouble();
}
}
read(archive, "zmp", zmp);
body->calcForwardKinematics();
setCurrentBaseLink(body->link(archive.get("currentBaseLink", "")));
bool staticModel;
if(archive.read("staticModel", staticModel)){
onStaticModelPropertyChanged(staticModel);
}
archive.read("selfCollisionDetection", isSelfCollisionDetectionEnabled);
archive.read("isEditable", isEditable);
self->notifyKinematicStateChange();
}
return restored;
}
void ViewManager::restoreViews(ArchivePtr archive, const std::string& key, ViewManager::ViewStateInfo& out_viewStateInfo)
{
MessageView* mv = MessageView::instance();
typedef map<ViewInfo*, vector<View*> > ViewsMap;
ViewsMap remainingViewsMap;
Listing* viewList = archive->findListing(key);
if(viewList->isValid() && !viewList->empty()){
vector<ViewState>* viewsToRestoreState = new vector<ViewState>();
out_viewStateInfo.data = viewsToRestoreState;
int id;
string moduleName;
string className;
string instanceName;
for(int i=0; i < viewList->size(); ++i){
Archive* viewArchive = dynamic_cast<Archive*>(viewList->at(i)->toMapping());
if(viewArchive){
bool isHeaderValid =
viewArchive->read("id", id) &&
viewArchive->read("plugin", moduleName) &&
viewArchive->read("class", className);
if(isHeaderValid){
View* view = 0;
if(!viewArchive->read("name", instanceName)){
view = getOrCreateView(moduleName, className);
} else {
// get one of the view instances having the instance name, or create a new instance.
// Different instances are assigned even if there are instances with the same name in the archive
ViewInfo* info = findViewInfo(moduleName, className);
if(info){
vector<View*>* remainingViews;
ViewsMap::iterator p = remainingViewsMap.find(info);
if(p != remainingViewsMap.end()){
remainingViews = &p->second;
} else {
remainingViews = &remainingViewsMap[info];
InstanceInfoList& instances = info->instances;
remainingViews->reserve(instances.size());
InstanceInfoList::iterator q = instances.begin();
if(info->hasDefaultInstance() && q != instances.end()){
++q;
}
while(q != instances.end()){
remainingViews->push_back((*q++)->view);
}
}
for(vector<View*>::iterator q = remainingViews->begin(); q != remainingViews->end(); ++q){
if((*q)->name() == instanceName){
view = *q;
remainingViews->erase(q);
break;
}
}
if(!view){
if(!info->isSingleton() || info->instances.empty()){
view = info->createView(instanceName, true);
} else {
mv->putln(MessageView::ERROR,
boost::format(_("A singleton view \"%1%\" of the %2% type cannot be created because its singleton instance has already been created."))
% instanceName % info->className());
}
}
}
}
if(view){
archive->registerViewId(view, id);
ArchivePtr state = viewArchive->findSubArchive("state");
if(state->isValid()){
state->inheritSharedInfoFrom(*archive);
viewsToRestoreState->push_back(ViewState(view, state));
}
if(viewArchive->get("mounted", false)){
mainWindow->viewArea()->addView(view);
}
}
}
}
}
}
}