bool RobotModel::setJointPos(std::string jointName, double jointVal)
{
osg::ref_ptr<osg::Node> node = findNamedNode(jointName, original_root_);
if(!node)
return false;
osg::ref_ptr<OSGSegment> jnt = dynamic_cast<OSGSegment*>(node->getUserData());
jnt->setJointPos(jointVal);
return true;
}
/***********************************************************
findShapeDrawable
***********************************************************/
osg::ShapeDrawable* OsgObjectHandler::findShapeDrawable()
{
osg::ShapeDrawable* shpdraw = NULL;
if(_OsgObject)
{
osg::Node* node = findNamedNode("Colorable", _OsgObject);
if(node)
{
osg::Geode* geode = node->asGeode();
if(geode)
{
osg::Drawable* drawable = geode->getDrawable(0);
shpdraw = dynamic_cast<osg::ShapeDrawable*>(drawable);
}
}
}
if(!shpdraw)
{
if(_OsgObjectNoLight)
{
osg::Node* node = findNamedNode("Colorable", _OsgObjectNoLight);
if(node)
{
osg::Geode* geode = node->asGeode();
if(geode)
{
osg::Drawable* drawable = geode->getDrawable(0);
shpdraw = dynamic_cast<osg::ShapeDrawable*>(drawable);
}
}
}
}
return shpdraw;
}
osg::ref_ptr<OSGSegment> RobotModel::getSegment(std::string name)
{
osg::ref_ptr<osg::Node> node = findNamedNode(name, original_root_);
if(!node){
std::cerr << "Could not find segment with name: " << name << std::endl;
return 0;
}
osg::ref_ptr<OSGSegment> jnt = dynamic_cast<OSGSegment*>(node->getUserData());
if(!jnt){
std::cerr << "Could not retrieve user data from node "<<name<<std::endl;
}
assert(jnt);
return jnt;
}
static osg::Node* findNamedNode(const std::string& searchName, osg::Node* currNode)
{
osg::Group* currGroup;
osg::Node* foundNode;
// check to see if we have a valid (non-NULL) node.
// if we do have a null node, return NULL.
if ( !currNode)
{
return NULL;
}
// We have a valid node, check to see if this is the node we
// are looking for. If so, return the current node.
if (currNode->getName() == searchName)
{
return currNode;
}
// We have a valid node, but not the one we are looking for.
// Check to see if it has children (non-leaf node). If the node
// has children, check each of the child nodes by recursive call.
// If one of the recursive calls returns a non-null value we have
// found the correct node, so return this node.
// If we check all of the children and have not found the node,
// return NULL
currGroup = currNode->asGroup(); // returns NULL if not a group.
if ( currGroup )
{
for (unsigned int i = 0 ; i < currGroup->getNumChildren(); i ++)
{
foundNode = findNamedNode(searchName, currGroup->getChild(i));
if (foundNode)
return foundNode; // found a match!
}
return NULL; // We have checked each child node - no match found.
}
else
{
return NULL; // leaf node, no match
}
}
/**
* \brief Locate a named SSG node in a branch.
*
* This method locates a named node in an SSG scenegraph branch. The
* node name usually corresponds to the name of an object in the 3D model.
*
* This method was taken from SimGear, credits go to the original
* authors.
*
* \param node Pointer to the scenegraph branch
* \param name Name of the object to search for
* \return Pointer to the node or NULL if no node with this name was found
*/
ssgEntity* SSGUtil::findNamedNode (ssgEntity* node, const char* name)
{
std::string NodeName = SSGUtil::getPureNodeName(node);
if (NodeName == name)
{
return node;
}
else if (node->isAKindOf(ssgTypeBranch()))
{
int nKids = node->getNumKids();
for (int i = 0; i < nKids; i++)
{
ssgEntity * result =
findNamedNode(((ssgBranch*)node)->getKid(i), name);
if (result != 0)
{
return result;
}
}
}
return NULL;
}
