Ejemplo n.º 1
0
bool EditableModelItemImpl::loadModelFile(const std::string& filename)
{
    BodyPtr newBody;

    MessageView* mv = MessageView::instance();
    mv->beginStdioRedirect();

    bodyLoader.setMessageSink(mv->cout(true));
    newBody = bodyLoader.load(filename);

    mv->endStdioRedirect();
    
    if(newBody){
        newBody->initializeState();
        newBody->calcForwardKinematics();
        Link* link = newBody->rootLink();
        
        AbstractBodyLoaderPtr loader = bodyLoader.lastActualBodyLoader();
        VRMLBodyLoader* vloader = dynamic_cast<VRMLBodyLoader*>(loader.get());
        if (vloader) {
            // VRMLBodyLoader supports retriveOriginalNode function
            setLinkTree(link, vloader);
        } else {
            // Other loaders dont, so we wrap with inline node
            VRMLProtoInstance* proto = new VRMLProtoInstance(new VRMLProto(""));
            MFNode* children = new MFNode();
            VRMLInlinePtr inl = new VRMLInline();
            inl->urls.push_back(filename);
            children->push_back(inl);
            proto->fields["children"] = *children;
            // first, create joint item
            JointItemPtr item = new JointItem(link);
            item->originalNode = proto;
            self->addChildItem(item);
            ItemTreeView::instance()->checkItem(item, true);
            // next, create link item under the joint item
            LinkItemPtr litem = new LinkItem(link);
            litem->originalNode = proto;
            litem->setName("link");
            item->addChildItem(litem);
            ItemTreeView::instance()->checkItem(litem, true);
        }
        for (int i = 0; i < newBody->numDevices(); i++) {
            Device* dev = newBody->device(i);
            SensorItemPtr sitem = new SensorItem(dev);
            Item* parent = self->findItem<Item>(dev->link()->name());
            if (parent) {
                parent->addChildItem(sitem);
                ItemTreeView::instance()->checkItem(sitem, true);
            }
        }
    }

    return (newBody);
}
Ejemplo n.º 2
0
void Test_printFieldsOfBoxNode(CX3DParser& parser, char *vrmlFile)
{
	// ---------------------------------------------
	// ---------------------------------------------
	if (!parser.parse(vrmlFile))
	{
		fprintf(stderr, "%s parse failed\n", vrmlFile);
		return;
	}

	CX3DParser::printLog("**** Fields of BoxNode ****\n");

	// ---------------------------------------------
	// ---------------------------------------------
	MFNode *pShapeNodes = parser.searchNodesFromAllChildrenOfRoot("Shape");
	if (pShapeNodes)
	{
		for (int i=0; i<pShapeNodes->count(); i++)
		{
			CX3DShapeNode *pShape = (CX3DShapeNode *)(pShapeNodes->getNode(i));

			if (pShape)
			{
				// --------------------------------------------------------
				//
				// --------------------------------------------------------
				SFNode *geometry = pShape->getGeometry();
				CX3DNode *pNode = geometry->getNode();

				// ---------------------------------------------
				// ---------------------------------------------
				if (pNode && pNode->getNodeType() == BOX_NODE)
				{
					CX3DBoxNode *pBoxNode = (CX3DBoxNode *)pNode;

					// ---------------------------------------------
					// ---------------------------------------------

					SFVec3f *sz = pBoxNode->getSize();
					printf("size : (%f, %f, %f)\n", sz->x(), sz->y(), sz->z());

					// solid
					SFBool *solid = pBoxNode->getSolid();
					printf("solid : %s\n", solid->getValue() ? "TRUE" : "FALSE");
				}
			}
		}

		delete pShapeNodes;
		pShapeNodes = NULL;
	}
}
Ejemplo n.º 3
0
void CX3DHAnimSiteNode::print(int indent)
{
	FILE *fp = CX3DParser::getDebugLogFp();

	char *nodeName = getNodeName();
	if (nodeName)
	{
		float x, y, z, rot;
		int i, n;

		CX3DParser::printIndent(indent);
		fprintf(fp, "%s (%s)\n", nodeName, CX3DNode::getNodeTypeString(getNodeType()));

		CX3DParser::printIndent(indent+1);
		fprintf(fp, "children\n");
		MFNode *nodes = getChildren();
		n = nodes->count();
		for (i=0; i<n; i++)
		{
			CX3DNode *child = nodes->getNode(i);
			if (child)
			{
				child->print(indent+2);
			}
		}

		CX3DParser::printIndent(indent+1);
		getCenter()->getValue(x, y, z);
		fprintf(fp, "center : (%f %f %f)\n", x, y, z);

		CX3DParser::printIndent(indent+1);
		getRotation()->getValue(x, y, z, rot);
		fprintf(fp, "rotation : (%f %f %f)(%f)\n", x, y, z, rot);

		CX3DParser::printIndent(indent+1);
		getScale()->getValue(x, y, z);
		fprintf(fp, "scale : (%f %f %f)\n", x, y, z);

		CX3DParser::printIndent(indent+1);
		getScaleOrientation()->getValue(x, y, z, rot);
		fprintf(fp, "scaleOrientation : (%f %f %f)(%f)\n", x, y, z, rot);

		CX3DParser::printIndent(indent+1);
		getTranslation()->getValue(x, y, z);
		fprintf(fp, "translation : (%f %f %f)\n", x, y, z);

		CX3DParser::printIndent(indent+1);
		fprintf(fp, "name (%s)\n", m_name.getValue());
	}
}
Ejemplo n.º 4
0
void VRMLBodyLoaderImpl::readJointSubNodes(LinkInfo& iLink, MFNode& childNodes, const ProtoIdSet& acceptableProtoIds, const Affine3& T)
{
    for(size_t i = 0; i < childNodes.size(); ++i){
        bool doTraverse = false;
        VRMLNode* childNode = childNodes[i].get();
        if(!childNode->isCategoryOf(PROTO_INSTANCE_NODE)){
            doTraverse = true;
        } else {
            VRMLProtoInstance* protoInstance = static_cast<VRMLProtoInstance*>(childNode);
            int id = PROTO_UNDEFINED;
            const string& protoName = protoInstance->proto->protoName;
            ProtoInfoMap::iterator p = protoInfoMap.find(protoName);
            if(p == protoInfoMap.end()){
                doTraverse = true;
                childNode = protoInstance->actualNode.get();
            } else {
                id = p->second.id;
                if(!acceptableProtoIds.test(id)){
                    throw invalid_argument(str(format(_("%1% node is not in a correct place.")) % protoName));
                }
                if(isVerbose){
                    messageIndent += 2;
                }
                switch(id){
                case PROTO_JOINT:
                    if(!T.matrix().isApprox(Affine3::MatrixType::Identity())){
                        throw invalid_argument(
                            str(format(_("Joint node \"%1%\" is not in a correct place.")) % protoInstance->defName));
                    }
                    iLink.link->appendChild(readJointNode(protoInstance, iLink.link->Rs()));
                    break;
                case PROTO_SEGMENT:
                    readSegmentNode(iLink, protoInstance, T);
                    linkOriginalMap[iLink.link] = childNodes[i];
                    break;
                case PROTO_SURFACE:
                    readSurfaceNode(iLink, protoInstance, T);
                    break;
                case PROTO_DEVICE:
                    readDeviceNode(iLink, protoInstance, T);
                    break;
                default:
                    doTraverse = true;
                    break;
                }
                if(isVerbose){
                    messageIndent -= 2;
                }
            }
        }
        if(doTraverse && childNode->isCategoryOf(GROUPING_NODE)){
            VRMLGroup* group = static_cast<VRMLGroup*>(childNode);
            if(VRMLTransform* transform = dynamic_cast<VRMLTransform*>(group)){
                readJointSubNodes(iLink, group->getChildren(), acceptableProtoIds, T * transform->toAffine3d());
            } else {
                readJointSubNodes(iLink, group->getChildren(), acceptableProtoIds, T);
            }
        }
    }
}
void Test_printChildrenOfRoot(CX3DParser& parser, char *vrmlFile)
{
	// ---------------------------------------------
	//  VRMLファイルをパースする
	// ---------------------------------------------
	if (!parser.parse(vrmlFile))
	{
		fprintf(stderr, "%s parse failed\n", vrmlFile);
		return;
	}

	CX3DParser::printLog("**** Children of Root Node ****\n");

	// ---------------------------------------------
	//  ルート直下のノードを得る
	// ---------------------------------------------
	MFNode *nodes = parser.getChildrenOfRootNode();

	if (nodes)
	{
		// ---------------------------------------------
		//  ノード名のみ表示
		// ---------------------------------------------
		for (int i=0; i<nodes->count(); i++)
		{
			CX3DNode *node = nodes->getNode(i);

			if (node)
			{
				printf("%s\n", node->getNodeName());
			}
		}

		// ---------------------------------------------
		//  後始末
		// ---------------------------------------------
		delete nodes;
		nodes = NULL;
	}
}
Ejemplo n.º 6
0
void Test_printNodeName(CX3DParser& parser, char *vrmlFile)
{
	// ---------------------------------------------
	// ---------------------------------------------
	if (!parser.parse(vrmlFile))
	{
		fprintf(stderr, "%s parse failed\n", vrmlFile);
		return;
	}

	CX3DParser::printLog("**** Node names of (%s) ****\n", vrmlFile);

	// ---------------------------------------------
	// ---------------------------------------------
	MFNode *nodes = parser.getChildrenOfRootNode();

	if (nodes)
	{
		// ---------------------------------------------
		// ---------------------------------------------
		for (int i=0; i<nodes->count(); i++)
		{
			CX3DNode *node = nodes->getNode(i);

			if (node)
			{
				printf("%s\n", node->getNodeName());
			}
		}

		// ---------------------------------------------
		// ---------------------------------------------
		delete nodes;
		nodes = NULL;
	}
}
Ejemplo n.º 7
0
void ModelNodeSetImpl::extractChildNodes
(JointNodeSetPtr jointNodeSet, MFNode& childNodes, const ProtoIdSet acceptableProtoIds, const Matrix44& T)
{
    for(size_t i = 0; i < childNodes.size(); i++){
        VrmlNode* childNode = childNodes[i].get();
        const Matrix44* pT;
        if(childNode->isCategoryOf(GROUPING_NODE)){
            VrmlGroup* groupNode = static_cast<VrmlGroup*>(childNode);
            VrmlTransform* transformNode = dynamic_cast<VrmlTransform*>(groupNode);
            Matrix44 T2;
            if( transformNode ){
                Matrix44 Tlocal;
                calcTransformMatrix(transformNode, Tlocal);
                T2 = T * Tlocal;
                pT = &T2;
            } else {
                pT = &T;
            }
            extractChildNodes(jointNodeSet, groupNode->getChildren(), acceptableProtoIds, *pT);

        } else if(childNode->isCategoryOf(LIGHT_NODE)){
            jointNodeSet->lightNodes.push_back(std::make_pair(T,childNode));
        } else if(childNode->isCategoryOf(PROTO_INSTANCE_NODE)){

            VrmlProtoInstance* protoInstance = static_cast<VrmlProtoInstance*>(childNode);
            int id = PROTO_UNDEFINED;
            bool doTraverseChildren = false;
            ProtoIdSet acceptableChildProtoIds(acceptableProtoIds);

            const string& protoName = protoInstance->proto->protoName;
            ProtoNameToInfoMap::iterator p = protoNameToInfoMap.find(protoName);

            if(p == protoNameToInfoMap.end()){
                doTraverseChildren = true;
            } else {
                id = p->second.id;
                if(!acceptableProtoIds.test(id)){
                    throw ModelNodeSet::Exception(protoName + " node is not in a correct place.");
                }
            }

            messageIndent += 2;

            switch(id){
                
            case PROTO_JOINT:
                if(T != Matrix44::Identity())
                    throw ModelNodeSet::Exception(protoName + " node is not in a correct place.");
                jointNodeSet->childJointNodeSets.push_back(addJointNodeSet(protoInstance));
                break;
                
            case PROTO_SENSOR:
                if(T != Matrix44::Identity())
                    throw ModelNodeSet::Exception(protoName + " node is not in a correct place.");
                jointNodeSet->sensorNodes.push_back(protoInstance);
                putMessage(protoName + protoInstance->defName);
                break;
                
            case PROTO_HARDWARECOMPONENT:
                if(T != Matrix44::Identity())
                    throw ModelNodeSet::Exception(protoName + " node is not in a correct place.");
                jointNodeSet->hwcNodes.push_back(protoInstance);
                putMessage(protoName + protoInstance->defName);
                break;
                
            case PROTO_SEGMENT:
                {
                    jointNodeSet->segmentNodes.push_back(protoInstance);
                    jointNodeSet->transforms.push_back(T);
                    putMessage(string("Segment node ") + protoInstance->defName);

                    doTraverseChildren = true;
                    acceptableChildProtoIds.reset();
                    acceptableChildProtoIds.set(PROTO_SENSOR);
                }
                break;
                
            default:
                break;
            }

            if(doTraverseChildren){
                if (protoInstance->fields.count("children")){
                    MFNode& childNodes = protoInstance->fields["children"].mfNode();
                    extractChildNodes(jointNodeSet, childNodes, acceptableChildProtoIds, T);
                }
            }

            messageIndent -= 2;
        }
    }
}
void CX3DOpenHRPSegmentNode::print(int indent)
{
	FILE *fp = CX3DParser::getDebugLogFp();
	int nMax = CX3DParser::getMaxPrintElemsForMFField();
	bool bPartialPrint = false;

	char *nodeName = getNodeName();
	if (nodeName)
	{
		float x, y, z;
		MFNode *nodes;
		int i, n;

		CX3DParser::printIndent(indent);
		fprintf(fp, "%s (%s)\n", nodeName, CX3DNode::getNodeTypeString(getNodeType()));

		CX3DParser::printIndent(indent+1);
		fprintf(fp, "children\n");
		nodes = getChildren();
		n = nodes->count();
		for (i=0; i<n; i++)
		{
			CX3DNode *child = nodes->getNode(i);
			if (child)
			{
				child->print(indent+2);
			}
		}

		CX3DParser::printIndent(indent+1);
		fprintf(fp, "name (%s)\n", m_name.getValue());

		CX3DParser::printIndent(indent+1);
		fprintf(fp, "coord\n");
		CX3DNode *pCoord = getCoord()->getNode();
		if (pCoord) pCoord->print(indent+2);

		CX3DParser::printIndent(indent+1);
		getCenterOfMass()->getValue(x, y, z);
		fprintf(fp, "centerOfMass : (%f %f %f)\n", x, y, z);

		CX3DParser::printIndent(indent+1);
		fprintf(fp, "displacers\n");
		nodes = getDisplacers();
		n = nodes->count();
		for (i=0; i<n; i++)
		{
			CX3DNode *child = nodes->getNode(i);
			if (child)
			{
				child->print(indent+2);
			}
		}

		CX3DParser::printIndent(indent+1);
		fprintf(fp, "mass (%f)\n", getMass()->getValue());

		CX3DParser::printIndent(indent+1);
		n = m_momentsOfInertia.count();
		fprintf(fp, "momentsOfInertia [%d]\n", n);
		if ((nMax > 0) && (n > nMax)) { n = nMax; bPartialPrint = true; }
		else { bPartialPrint = false; }
		CX3DParser::printIndent(indent+2);
		fprintf(fp, "(");
		for (i=0; i<n; i++)
		{
			fprintf(fp, "%f ", m_momentsOfInertia.getValue(i));
		}
		if (bPartialPrint) fprintf(fp, "...");
		fprintf(fp, ")\n");
	}
}
Ejemplo n.º 9
0
void CX3DOpenHRPHumanoidNode::print(int indent)
{
	FILE *fp = CX3DParser::getDebugLogFp();
	int nMax = CX3DParser::getMaxPrintElemsForMFField();
	bool bPartialPrint = false;

	char *nodeName = getNodeName();
	if (nodeName)
	{
		float x, y, z, rot;
		MFNode *nodes;
		int i, n;

		CX3DParser::printIndent(indent);
		fprintf(fp, "%s (%s)\n", nodeName, CX3DNode::getNodeTypeString(getNodeType()));

		CX3DParser::printIndent(indent+1);
		getCenter()->getValue(x, y, z);
		fprintf(fp, "center : (%f %f %f)\n", x, y, z);

		CX3DParser::printIndent(indent+1);
		getRotation()->getValue(x, y, z, rot);
		fprintf(fp, "rotation : (%f %f %f)(%f)\n", x, y, z, rot);

		CX3DParser::printIndent(indent+1);
		getScale()->getValue(x, y, z);
		fprintf(fp, "scale : (%f %f %f)\n", x, y, z);

		CX3DParser::printIndent(indent+1);
		getScaleOrientation()->getValue(x, y, z, rot);
		fprintf(fp, "scaleOrientation : (%f %f %f)(%f)\n", x, y, z, rot);

		CX3DParser::printIndent(indent+1);
		getTranslation()->getValue(x, y, z);
		fprintf(fp, "translation : (%f %f %f)\n", x, y, z);

		CX3DParser::printIndent(indent+1);
		fprintf(fp, "name (%s)\n", m_name.getValue());

		CX3DParser::printIndent(indent+1);
		n = m_info.count();
		fprintf(fp, "info [%d]\n", n);
		if ((nMax > 0) && (n > nMax)) { n = nMax; bPartialPrint = true; }
		else { bPartialPrint = false; }
		for (i=0; i<n; i++)
		{
			CX3DParser::printIndent(indent+2);
			fprintf(fp, "%s\n", m_info.getValue(i));
		}
		if (bPartialPrint)
		{
			CX3DParser::printIndent(indent+2);
			fprintf(fp, "...\n");
		}

		CX3DParser::printIndent(indent+1);
		nodes = getJoints();
		n = nodes->count();
		fprintf(fp, "joints [%d]\n", n);
		for (i=0; i<n; i++)
		{
			CX3DNode *child = nodes->getNode(i);
			if (child)
			{
				child->print(indent+2);
			}
		}

		CX3DParser::printIndent(indent+1);
		nodes = getSegments();
		n = nodes->count();
		fprintf(fp, "segments [%d]\n", n);
		for (i=0; i<n; i++)
		{
			CX3DNode *child = nodes->getNode(i);
			if (child)
			{
				child->print(indent+2);
			}
		}

		CX3DParser::printIndent(indent+1);
		nodes = getSites();
		n = nodes->count();
		fprintf(fp, "sites [%d]\n", n);
		for (i=0; i<n; i++)
		{
			CX3DNode *child = nodes->getNode(i);
			if (child)
			{
				child->print(indent+2);
			}
		}

		CX3DParser::printIndent(indent+1);
		nodes = getHumanoidBody();
		n = nodes->count();
		fprintf(fp, "humanoidBody [%d]\n", n);
		for (i=0; i<n; i++)
		{
			CX3DNode *child = nodes->getNode(i);
			if (child)
			{
				child->print(indent+2);
			}
		}


		CX3DParser::printIndent(indent+1);
		fprintf(fp, "version (%s)\n", m_version.getValue());

		CX3DParser::printIndent(indent+1);
		nodes = getViewpoints();
		n = nodes->count();
		fprintf(fp, "viewpoints [%d]\n", n);
		for (i=0; i<n; i++)
		{
			CX3DNode *child = nodes->getNode(i);
			if (child)
			{
				child->print(indent+2);
			}
		}
	}
}
Ejemplo n.º 10
0
void H3DViewerPopupMenus::OnTreeViewAddChildNode( wxCommandEvent& event ) {
  wxTreeItemId id = treeview_dialog->TreeViewTree->GetSelection();
  
  H3DViewerTreeViewDialog::TreeIdMap::iterator ni = treeview_dialog->node_map.find( id.m_pItem );
  if( ni == treeview_dialog->node_map.end() ) {
    wxMessageBox( wxT("Selected tree item is not a node"),
                  wxT("Error"),
                  wxOK | wxICON_EXCLAMATION);
  }

  vector< wxString > node_fields;

  Node *selected_node = (*ni).second.get();
  H3DNodeDatabase *db = H3DNodeDatabase::lookupNodeInstance( selected_node );
  for( H3DNodeDatabase::FieldDBConstIterator i = db->fieldDBBegin();
       db->fieldDBEnd() != i; ++i ) {
    Field *f = i.getField( selected_node ); 
    if( SFNode *sfnode = dynamic_cast< SFNode * >( f ) ) {
      if( sfnode->getAccessType() == Field::INPUT_ONLY ||
          sfnode->getAccessType() == Field::INPUT_OUTPUT ) {
        node_fields.push_back( wxString(sfnode->getName().c_str(),wxConvUTF8) );
      } 
    } else if( MFNode *mfnode = dynamic_cast< MFNode * >( f ) ) {
      if( mfnode->getAccessType() == Field::INPUT_ONLY ||
          mfnode->getAccessType() == Field::INPUT_OUTPUT ) {
        node_fields.push_back( wxString(mfnode->getName().c_str(), wxConvUTF8) );
      }
    }
  }
  
  if( node_fields.empty() ) {
    wxMessageBox( wxT("Selected node does not have a SFNode or MFNode field."),
                  wxT("Error"),
                  wxOK | wxICON_EXCLAMATION);
    return;
  }

  string field_to_change;

  if( node_fields.size() > 1 ) {
    wxString *choices = new wxString[ node_fields.size() ];
    for( unsigned int i = 0; i < node_fields.size(); ++i ) {
      choices[i] = node_fields[i];
    }
    wxSingleChoiceDialog *choice_dialog = new wxSingleChoiceDialog ( this,
                                                                     wxT("Add/replace node in which field.."),
                                                                     wxT(""),
                                                                     node_fields.size(),
                                                                     choices );
    delete [] choices;

    if (choice_dialog->ShowModal() == wxID_OK) {
      field_to_change = std::string( node_fields[ choice_dialog->GetSelection() ].mb_str() );
    } else {
      return;
    }
  }

  wxTextEntryDialog *node_name_dialog = 
    new wxTextEntryDialog(this, 
                          wxT("Enter the name of the node type you want to use" ),
                          wxT("Add/replace node" ) );
  if (node_name_dialog->ShowModal() == wxID_OK) {
    Node *new_node = H3DNodeDatabase::createNode( std::string(node_name_dialog->GetValue().mb_str()) );
    if( !new_node ) {
      wxMessageBox( wxT("No such node type exists: " + node_name_dialog->GetValue()),
                    wxT("Error"),
                    wxOK | wxICON_EXCLAMATION);
    } else {
      Field *f = selected_node->getField( field_to_change );
      SFNode *sfnode = dynamic_cast< SFNode * >( f );
      MFNode *mfnode = dynamic_cast< MFNode * >( f );
       
      if( sfnode ) {
        AutoRef< Node > old_node( sfnode->getValue() );
        try { 
          sfnode->setValue( new_node );
        } catch (...) {
          sfnode->setValue( old_node.get() );
          wxMessageBox( wxT("Invalid node type for field"),
                        wxT("Error"),
                        wxOK | wxICON_EXCLAMATION);
        }
      } else if( mfnode ) {
        try { 
          mfnode->push_back( new_node );
        } catch (...) {
          wxMessageBox( wxT("Invalid node type for field"),
                        wxT("Error"),
                        wxOK | wxICON_EXCLAMATION);
        }
      }
    }
  } 
}