scenecomponents.cpp

来自「3D仿真组实物机器人环境下的机器人模型的设计工具。可以查看和修改现有模型的详细参」· C++ 代码 · 共 2,568 行 · 第 1/5 页

 * This method parses three coordinates from the string given and returns them as VecPosition
 * @param str String that is going to be parsed for coordinates
 * @return The vecposition containing the coordinates in string
 */
VecPosition TSceneNode::parsePositions( string & str, unsigned & index )
{
  if( !Parse::gotoFirstParsableModule( str, index ) )
  {
    cerr << "TSceneNode::parsePosition: Couldn't get any coordinates, end of string" << endl;
    return false;
  }

  string token1, token2, token3;

  if( !Parse::getToken( str, index, token1 ) || token1 == ")" )
  {
    cerr << "TSceneNode::parsePosition: Couldn't get first coordinate" << endl;
    return false;
  }

  if( !Parse::getToken( str, index, token2 ) || token2 == ")" )
  {
    cerr << "TSceneNode::parsePosition: Couldn't get second coordinate" << endl;
    return false;
  }

  if( !Parse::getToken( str, index, token3 ) || token3 == ")" )
  {
    cerr << "TSceneNode::parsePosition: Couldn't get third coordinate" << endl;
    return false;
  }

  return 
        VecPosition( 
           TGenericData::getNumberFromString( token1 ),
           TGenericData::getNumberFromString( token2 ),
           TGenericData::getNumberFromString( token3 )
                   );
}

/**
 * Thismethod parses and initializes the node with given string that contains information
 * needed for this process
 * @param str String containing information needed to initialize this node
 * @param parrent Parrent to stick this child to
 * @param nodes List that contains all of the nodes parsed till here
 * @return Indicates that parse and load was successfull
 */
bool TSceneNode::parseAndLoad( string str, TSceneNode * parrent, TNodeList & nodes, TJointList &joints )
{
  unsigned index = 0;

  if( !Parse::gotoFirstParsableModule( str, index ) || str[index] == ')' )
  {
    cerr << "TSceneNode::parseAndLoad: Node informations truncted in begining" << endl;
    return false;
  }
  else if( str.compare( index + 4, NODE_CCYLINDER.length(), NODE_CCYLINDER ) == 0 )
  {
    if( !parseCylinderNode( str, parrent, nodes, joints ) && bStrict )
      return false;
  }
  else if( str.compare( index + 4, NODE_BOX.length(), NODE_BOX ) == 0 )
  {
    if( !parseBoxNode( str, parrent, nodes, joints ) && bStrict )
      return false;
  }
  else if( str.compare( index + 4, NODE_TRANSFORM.length(), NODE_TRANSFORM ) == 0 )
  {
    if( !parseTransformNode( str, parrent, nodes, joints ) && bStrict )
      return false;
  }
  else if( str.compare( index + 4, NODE_SPACE.length(), NODE_SPACE ) == 0 )
  {
    if( !parseSpaceNode( str, parrent, nodes, joints ) )
      return false;
  }
  else if( str.compare( index + 4, NODE_AGENTASPECT.length(), NODE_AGENTASPECT ) == 0 )
  {
    if( !parseAgentAspectNode( str, parrent, nodes, joints ) )
      return false;
  }
  else
  {
     cerr << "TSceneNode::parseAndLoad: Unknown string to parse: '" << str << "'" << endl;
     return false;
  }

  m_Parrent = parrent;
  nodes.push_back( this );

  #ifdef VERBOSE
  cout << "TSceneNode: Adding new node : " << this << endl;
  #endif

  return true;
}

/**
 * This method adds this joint and all of its children to the list specified,
 * so that some procedure can be done on every joint in scene.
 * @param nodes List that should contain all joints
 */
void TSceneNode::queryNodes( TNodeList & nodes )
{
  nodes.push_back( this );
  for( unsigned i = 0; i < m_Children.size(); i++ )
    m_Children[i]->queryNodes( nodes );
}

/**
 * This method adds this joint and all of its children to the list specified,
 * so that some procedure can be done on every joint in scene.
 * @param nodes List that should contain all joints
 */
void TSceneNode::queryJoints( TJointList & joints )
{
  unsigned i, index;
  bool add;

  for( index = 0; index < m_Joints.size(); index++ )
  {
    add = true;
    for( i = 0; i < joints.size(); i++ )
      if( joints[i] == m_Joints[index] )
      {
        add = false;
        break;
      }

    if( add )
      joints.push_back( m_Joints[i] );
  }

  for( unsigned i = 0; i < m_Children.size(); i++ )
    m_Children[i]->queryJoints( joints );
}

/**
 * This metod returns the nodes parent
 * @return Parent of current node
 */
TSceneNode * TSceneNode::getParrent() const
{
  return m_Parrent;
}

/**
 * This method sets parrent of a node to the specified one
 * @param parrent 
 */
void TSceneNode::setParrent( TSceneNode * parrent )
{
  OpenGLEngine->removeObject(this);
  if( m_Parrent != NULL )
    m_Parrent->eraseChild( this );    

  if( !parrent )
    OpenGLEngine->addObject(this);
  else
    parrent->addChild( this );

  m_Parrent = parrent;
}

/**
 * This method adds a specific child to this node
 * @param child Child to be added
 */
void TSceneNode::addChild( TSceneNode * child )
{
  if( child )
    m_Children.push_back( child );
}

/**
 * This method ...
 */
void TSceneNode::clearChildren(TNodeList & nodes)
{
  for( unsigned i = 0; i < m_Children.size(); i++ )
    {
      m_Children[i]->clearChildren(nodes);

      for( unsigned j = 0; j < nodes.size(); j++ )
        if( nodes[j] == m_Children[i] )
        {
          nodes.erase( nodes.begin() + j );
          break;
        }

      delete m_Children[i];
    }

  m_Children.clear();
}

/**
 * This method removes a specific child from the list of children
 * @param child Child to be removed
 */
void TSceneNode::removeChild( TNodeList & nodes, TSceneNode * child )
{
  for( unsigned i = 0; i < m_Children.size(); i++ )
    if( m_Children[i] == child )
    {
      child->clearChildren( nodes );
      for( unsigned j = 0; j < nodes.size(); j++ )
        if( nodes[j] == child )
          nodes.erase( nodes.begin() + j );
      m_Children.erase( m_Children.begin() + i );
      delete child;
    }
}

/**
 * This method removes a specific child from the list of children
 * @param child Child to be removed
 */
void TSceneNode::eraseChild( TSceneNode * child )
{
  for( unsigned i = 0; i < m_Children.size(); i++ )
    if( m_Children[i] == child )
      m_Children.erase( m_Children.begin() + i );
}

/**
 * This method ...
 * @param os 
 */
void TSceneNode::saveToStream( ostream & os, string strGap ) const
{
  if( !initialized() )
    cerr << "TSceneNode::saveToStream: This node has not been initialized" << endl;

  m_Saved = true;
  if( m_Name != "" )
    os << endl << strGap << "; Setting up '" << m_Name << "'\n";
  os << strGap << "(";
  if( getNodeName() != "" )
    os << "nd " << getNodeName() << " \n";

  unsigned i;

  saveInformation( os, strGap + "    " );
  for( i = 0; i < m_Children.size(); i++ )
    m_Children[i]->saveToStream( os, strGap + "    " );

  for( i = 0; i < m_Joints.size(); i++ )
    m_Joints[i]->saveToStream( os, strGap + "    ", this );

  os << strGap << ")\n\n";
}

/**
 * Overloaded version of the C++ output operator for TGenericDatas.
 * This operator makes it possible to use TSceneNode in output
 * statements (e.g.  cout << data). The node name and shape are
 * printed in the format '(name,shape)'.
 * @param os Output stream which data is being printed
 * @param data Data which is being printed
 * @return Output stream instance to allow furthor prints
 */
ostream& operator <<( ostream &os, const TSceneNode & data )
{
  return ( data.show(os) );
}

/**
 * Overloaded version of the C++ output operator for TGenericDatas.
 * This operator makes it possible to use TGenericDatas in output
 * statements (e.g.  cout << data). The data name and values are
 * printed in the format '(name,value)'.
 * @param os Output stream which data is being printed
 * @param data Pointer to data which is being printed
 * @return Output stream instance to allow furthor prints
 */
ostream& operator <<( ostream &os, const TSceneNode * data )
{
  return ( os << (*data) );
}

/**
 * This method logs the TSceneNode class to the specified output stream
 * @param os outputstream to log the information to
 * @return the output stream instance
 */
ostream & TSceneNode::show( ostream & os ) const
{
  os << "(node " << m_NodeName << " '" << getName() << "' ";

  if( m_NodeName == NODE_SPACE || m_NodeName == NODE_AGENTASPECT )
    os << ")";
  else
  {
    switch( m_Shape )
    {
    case SHAPE_SPHERE:
      os << "sphere " << m_Radius;
      break;

    case SHAPE_BOX:
      os << "box " << m_Length << " " << m_Width << " " << m_Height;
      break;

    case SHAPE_CAPSULE:
      os << "capsule " << m_Length << " " << m_Radius;
      break;

    default:
      os << "illegal";
    }
    os << " " << getMass() << " " << getMaterial() << ")";
  }

  return os;
}



/*********************************************************************************************/
/***********************************  Class TSceneJoint  *************************************/
/*********************************************************************************************/

/**
 * This is the constructor of TSceneJoint
 */
TSceneJoint::TSceneJoint()
{
  m_Joint = JOINT_ILLEGAL;
  m_MaxMotorForce[ 0 ] = 0;
  m_MaxMotorForce[ 1 ] = 0;
  m_Parrent = NULL;
  m_Child = NULL;
  m_Radius = 0.3;                                
  m_Saved = false;
  m_Renderable = false;
  m_DiffuseColor = TRGBA( 153,153,153, 1 );
}

/**
 * This is the destructor of the TSceneJoint
 */
TSceneJoint::~TSceneJoint()
{
  if( m_Parrent != NULL )
    m_Parrent->removeJoint( this );

  if( m_Child != NULL )
    m_Child->removeJoint( this );

  if( OpenGLEngine )
    OpenGLEngine->removeObject( this );
}


/**
 * This method checks if the supplied string is an effector variable
 * @param str String to check for effector in
 * @return True if any kind of effector resides in it
 */
bool TSceneJoint::isEffector( string & str ) const
{
  if( str.compare( 4, EFFECTOR_HINGE.length(), EFFECTOR_HINGE ) == 0 ||
      str.compare( 4, EFFECTOR_UNIVERSAL.length(), EFFECTOR_UNIVERSAL ) == 0 ||
      str.compare( 4, EFFECTOR_HINGE2.length(), EFFECTOR_HINGE2 ) == 0 )
    return true;
  else
    return false;
}

/**
 * This method parses a string for effector name data to pack into lists
 * @param str String that contains the information for the effector
 * @param joint Joint instance to write the information to
 * @return Indicates wheather information is parsed successfully
 */
bool TSceneJoint::parseEffector( string & str )
{
  if( !isEffector( str ) )
  {
    cerr << "TSceneJoint::parseEffector: This is not an effector" << endl;
    return false;
  }

  unsigned index = EFFECTOR_UNIVERSAL.length() + 4;
  if( str.compare( 4, EFFECTOR_HINGE.length(), EFFECTOR_HINGE ) == 0 )
    index = EFFECTOR_HINGE.length() + 4;
  else if( str.compare( 4, EFFECTOR_HINGE2.length(), EFFECTOR_HINGE2 ) == 0 )
    index = EFFECTOR_HINGE2.length() + 4;

  if( !Parse::gotoFirstParsableModule( str, index ) )
  {
    cerr << "TSceneJoint::parseEffector: Effector is truncated" << endl;
    return false;
  }

  if( str.compare( index, CMD_NAME.length(), CMD_NAME ) != 0 )
  {
    cerr << "TSceneJoint::parseEffector: Effector does not have a 'setName' compound" << endl;
    return false;
  }

  index += CMD_NAME.length();
  string token;
  if( !Parse::getToken( str, index, token ) )
    return false;

  setEffectorName( token );
  return true;
}

/**
 * This method checks if the supplied string is an perceptor variable
 * @param str String to check for perceptor in
 * @return True if any kind of perceptor resides in it
 */
bool TSceneJoint::isPerceptor( string & str ) const
{
  if( str.compare( 4, PERCEPT_HINGE.length(), PERCEPT_HINGE ) == 0 ||
      str.compare( 4, PERCEPT_UNIVERSAL.length(), PERCEPT_UNIVERSAL ) == 0 ||
      str.compare( 4, PERCEPT_HINGE2.length(), PERCEPT_HINGE2 ) == 0 )
    return true;
  else
    return false;
}

/**
 * This method parses a string for perceptor name data to pack into lists
 * @param str String that contains the information for the perceptor
 * @return Indicates wheather information is parsed successfully
 */
bool TSceneJoint::parsePerceptor( string & str)
{
  if( !isPerceptor( str ) )
  {
    cerr << "TSceneJoint::parsePerceptor: This is not a perceptor" << endl;
    return false;
  }

  unsigned index = PERCEPT_UNIVERSAL.length() + 4;
  if( str.compare( 4, PERCEPT_HINGE.length(), PERCEPT_HINGE ) == 0 )
    index = PERCEPT_HINGE.length() + 4;
  else if( str.compare( 4, PERCEPT_HINGE2.length(), PERCEPT_HINGE2 ) == 0 )
    index = PERCEPT_HINGE2.length() + 4;

  if( !Parse::gotoFirstParsableModule( str, index ) )
  {
    cerr << "TSceneJoint::parsePerceptor: Perceptor is truncated" << endl;
    return false;
  }

  if( str.compare( index, CMD_NAME.length(), CMD_NAME ) != 0 )
  {
    cerr << "TSceneJoint::parsePerceptor: Perceptor does not have a 'setName' compound" << endl;
    return false;
  }

  index += CMD_NAME.length();
  string token;
  if( !Parse::getToken( str, index, token ) )
    return false;

  setPerceptorName( token );
  return true;
}

/**
 * This method attaches two nodes together via a joint. Information needed
 * for this is stored in the string supplied for the function.
 * Example: '(attach ../boxBody ../../rightshank/boxBody)'
 * @param str String containing information needed for attach
 * @param node Node to connect the joint to
 * @param list List of all the nodes created before
 * @return Indicates that successfully attached two nodes
 */
bool TSceneJoint::parseAttach( string & str, TSceneNode * parrent, TNodeList & list )
{
  if( str.compare( 0, CMD_ATTACH.length(), CMD_ATTACH ) != 0 )
  {
    cerr << "TSceneJoint::parseAttach: This is not an attach command" << endl;
    return false;
  }

  string self, child;
  unsigned index = CMD_ATTACH.length();
  unsigned index1, index2;

  /// Parent info
  if( !Parse::getToken( str, index, self ) )
  {
    cerr << "TSceneJoint::parseAttach: Could not parse self address" << endl;
    return false;
  }

  index1 = self.rfind( "/" );
  if( index1 == self.npos || index1 == 0 )
  {
    cerr << "TSceneJoint::parseAttach: Could not get self name (1): index1 = " << index1 << endl;
    return false;
  }

  index2 = self.rfind( "/", index1 - 1 );
  if( index2 == self.npos )
    index2 = -1;
  self = self.substr( index2 + 1, index1 - index2 - 1 );

  /// Child info
  if( !Parse::getToken( str, index, child ) )
  {
    cerr << "TSceneJoint::parseAttach: Could not parse parrent position" << endl;
    return false;
  }