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);
}
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;
}
}
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());
}
}
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;
}
}
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;
}
}
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");
}
}
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);
}
}
}
}
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);
}
}
}
}
}