void LightManip(SoSeparator * root)
{

  SoInput in;
  in.setBuffer((void *)scenegraph, std::strlen(scenegraph));
  SoSeparator * _root = SoDB::readAll( &in );
  if ( _root == NULL ) return; // Shouldn't happen.
  root->addChild(_root);
  root->ref();

  const char * pointlightnames[3] = { "RedLight", "GreenLight", "BlueLight" };
  SoSearchAction sa;

  for (int i = 0; i < 3; i++) {
    sa.setName( pointlightnames[i] );
    sa.setInterest( SoSearchAction::FIRST );
    sa.setSearchingAll( false );
    sa.apply( root );
    SoPath * path = sa.getPath();
    if ( path == NULL) return; // Shouldn't happen.

    SoPointLightManip * manip = new SoPointLightManip;
    manip->replaceNode( path );
  }


} 
Esempio n. 2
0
SoNodeList Renderer::getChildByName(SoSeparator * ivRoot, SbName & childName,
                                            SoType targetType, int maxResultsExpected)
{
    assert(ivRoot);
    SoNodeList resultList;

    SoSearchAction sa;
    sa.setSearchingAll(true);

    sa.setType(targetType, true);
    sa.setInterest( SoSearchAction::ALL);
    sa.setName(childName);
    sa.setFind(SoSearchAction::NAME);
    sa.apply(ivRoot);

    SoPathList &pathList = sa.getPaths();
    int numPaths = pathList.getLength();
    if (numPaths > maxResultsExpected)
    {
        //DBGA(this->className() << "::getChildByName::Found too many children of node: "
        //     << ivRoot->getName().getString()  << " with name: "
        //     <<childName.getString() << " " );
        //DBGA(this->className() << "::getChildByName:: Expected:" << maxResultsExpected
        //     << " Found:" << numPaths);
        //resultList.append(static_cast<SoNode *>(NULL));
        return resultList;
    }

    for(int i = 0; i < numPaths; ++i)
    {
        resultList.append(pathList[i]->getTail());
    }
    return resultList;
}
Esempio n. 3
0
int QilexDoc::doc_insert_kinematic_chain(Rchain *kineengine, SoSeparator *kinechain)
{
   int error = 0;
   int i;

   SbVec3f joinax;

   float joinangle;
   
   SbName joints[] = {"joint1", "joint2", "joint3", "joint4","joint5", "joint6",
                   "joint7", "joint8", "joint9", "joint10","joint11", "joint12",
                   "joint13", "joint14", "joint15", "joint16","joint17", "joint18",
                   "joint19", "joint20", "joint21", "joint22","joint23", "joint24",  };
   
   SoEngineList compR(kineengine->dof);
   SoNodeList Rots(kineengine->dof);

   SoSearchAction lookingforjoints;
   SoTransform *pjoint = new SoTransform;

      // Identifie the rotations and assing the job
   i = 0;

   while (i < kineengine->dof && error == 0)
   {
      lookingforjoints.setName(joints[i]);
      lookingforjoints.setType(SoTransform::getClassTypeId());
      lookingforjoints.setInterest(SoSearchAction::FIRST);
      lookingforjoints.apply(kinechain);

      //  assert(lookingforjoints.getPath() != NULL);
      SoNode * pnode = lookingforjoints.getPath()->getTail();;

      pjoint = (SoTransform *) pnode;

      if(NULL != pjoint)
      {
         Rots.append((SoTransform *) pjoint);
         compR.append(new SoQtComposeRotation);
         // cal comprobar si l'articulació es de rotació o translació: arreglar...
         ((SoTransform *) Rots[i])->rotation.getValue(joinax, joinangle);
         ((SoQtComposeRotation *) compR[i])->axis.setValue(joinax);

         ((SoTransform *) Rots[i])->rotation.connectFrom(&((SoQtComposeRotation *) compR[i])->rotation);
      }
      else
      {
         error = 5; // not a valid Model3d file
      }
      i++ ;
   }

   if (error == 0)
   {
      SoSeparator *axisworld = new SoSeparator;
      axisworld->unrefNoDelete();
		
		SoCoordinateAxis *AxisW   = new SoCoordinateAxis();
	   AxisW->fNDivision = 1;
      AxisW->fDivisionLength = 200;
		axisworld->addChild(AxisW);

      //kinechain->insertChild(AxisW,1);
      view->addNoColObject(axisworld);
		
    /*  lookingforjoints.setName("tool");
      lookingforjoints.setType(SoSeparator::getClassTypeId());
      lookingforjoints.setInterest(SoSearchAction::FIRST);
      lookingforjoints.apply(kinechain);

      if(lookingforjoints.getPath() != NULL)
      {
         SoNode * pnode = lookingforjoints.getPath()->getTail();;
      	
			SoCoordinateAxis *AxisT   = new SoCoordinateAxis();
			AxisT->fNDivision = 1;
      	AxisT->fDivisionLength = 200;

			SoSeparator *axistool = new SoSeparator;
         axistool->ref();
         axistool = (SoSeparator *) pnode;
         axistool->addChild(AxisT);
			view->addNoColObject(axistool);			
      }
      */ 
      panel_control *panel = new panel_control(0, "Panel", kineengine);

      for (int i = 0; i < kineengine->dof; i++)
      {
         // connect(panel->ldial[i], SIGNAL(valueChange(double)),((SoQtComposeRotation *) compR[i]), SLOT(setValue_angle(double)));
         connect(panel->ldial[i], SIGNAL(valueChange(double)),&panel->kinechain->list_plug[i], SLOT(setValue(double)));
         connect(&panel->kinechain->list_plug[i], SIGNAL(valueChanged(double)),((SoQtComposeRotation *) compR[i]), SLOT(setValue_angle(double)));
         connect(&panel->kinechain->list_plug[i], SIGNAL(valueChanged(double)),panel->ldial[i], SLOT(setValue(double)));
      }

      view->addRobotCell(kinechain);

      panel->show();
      panel->update_limits();
      //panel->kinechain->setconsole_mode(true); Ja estava comentada
      panel->kinechain->setconsole_mode(false);
      panel->kinechain->do_ready();
		connect(view, SIGNAL(pick_point(Rhmatrix)),panel, SLOT(move_pickpoint(Rhmatrix )));
      //panel->kinechain->setconsole_mode(false); Ja estava comentada
   }
void ViewProviderRobotObject::updateData(const App::Property* prop)
{
    Robot::RobotObject* robObj = static_cast<Robot::RobotObject*>(pcObject);
    if (prop == &robObj->RobotVrmlFile) {
        // read also from file
        const char* filename = robObj->RobotVrmlFile.getValue();
        QString fn = QString::fromUtf8(filename);
        QFile file(fn);
        SoInput in;
        pcRobotRoot->removeAllChildren();
        if (!fn.isEmpty() && file.open(QFile::ReadOnly)) {
            QByteArray buffer = file.readAll();
            in.setBuffer((void *)buffer.constData(), buffer.length());
            SoSeparator * node = SoDB::readAll(&in);
            if (node) pcRobotRoot->addChild(node);
            pcRobotRoot->addChild(pcTcpRoot);
        }
		// search for the conection points +++++++++++++++++++++++++++++++++++++++++++++++++
		Axis1Node = Axis2Node = Axis3Node = Axis4Node = Axis5Node = Axis6Node = 0;
		SoSearchAction searchAction;
		SoPath * path;

		// Axis 1
		searchAction.setName("FREECAD_AXIS1");
		searchAction.setInterest(SoSearchAction::FIRST);
		searchAction.setSearchingAll(FALSE);
		searchAction.apply(pcRobotRoot);
		path = searchAction.getPath();
		if(path){
			SoNode* node = path->getTail();
			std::string typeName = (const char*)node->getTypeId().getName();
			if (!node || node->getTypeId() != SoVRMLTransform::getClassTypeId())
				throw; // should not happen
			Axis1Node = static_cast<SoVRMLTransform *>(node);
		}
		// Axis 2
		searchAction.setName("FREECAD_AXIS2");
		searchAction.setInterest(SoSearchAction::FIRST);
		searchAction.setSearchingAll(FALSE);
		searchAction.apply(pcRobotRoot);
		path = searchAction.getPath();
		if(path){
			SoNode* node = path->getTail();
			std::string typeName = (const char*)node->getTypeId().getName();
			if (!node || node->getTypeId() != SoVRMLTransform::getClassTypeId())
				throw; // should not happen
			Axis2Node = static_cast<SoVRMLTransform *>(node);
		}
		// Axis 3
		searchAction.setName("FREECAD_AXIS3");
		searchAction.setInterest(SoSearchAction::FIRST);
		searchAction.setSearchingAll(FALSE);
		searchAction.apply(pcRobotRoot);
		path = searchAction.getPath();
		if(path){
			SoNode* node = path->getTail();
			std::string typeName = (const char*)node->getTypeId().getName();
			if (!node || node->getTypeId() != SoVRMLTransform::getClassTypeId())
				throw; // should not happen
			Axis3Node = static_cast<SoVRMLTransform *>(node);
		}
		// Axis 4
		searchAction.setName("FREECAD_AXIS4");
		searchAction.setInterest(SoSearchAction::FIRST);
		searchAction.setSearchingAll(FALSE);
		searchAction.apply(pcRobotRoot);
		path = searchAction.getPath();
		if(path){
			SoNode* node = path->getTail();
			std::string typeName = (const char*)node->getTypeId().getName();
			if (!node || node->getTypeId() != SoVRMLTransform::getClassTypeId())
				throw; // should not happen
			Axis4Node = static_cast<SoVRMLTransform *>(node);
		}
		// Axis 5
		searchAction.setName("FREECAD_AXIS5");
		searchAction.setInterest(SoSearchAction::FIRST);
		searchAction.setSearchingAll(FALSE);
		searchAction.apply(pcRobotRoot);
		path = searchAction.getPath();
		if(path){
			SoNode* node = path->getTail();
			std::string typeName = (const char*)node->getTypeId().getName();
			if (!node || node->getTypeId() != SoVRMLTransform::getClassTypeId())
				throw; // should not happen
			Axis5Node = static_cast<SoVRMLTransform *>(node);
		}
		// Axis 6
		searchAction.setName("FREECAD_AXIS6");
		searchAction.setInterest(SoSearchAction::FIRST);
		searchAction.setSearchingAll(FALSE);
		searchAction.apply(pcRobotRoot);
		path = searchAction.getPath();
		if(path){
			SoNode* node = path->getTail();
			std::string typeName = (const char*)node->getTypeId().getName();
			if (!node || node->getTypeId() != SoVRMLTransform::getClassTypeId())
				throw; // should not happen
			Axis6Node = static_cast<SoVRMLTransform *>(node);
		}
		if(Axis1Node)
			Axis1Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis1.getValue()*(M_PI/180));
		if(Axis2Node)
			Axis2Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis2.getValue()*(M_PI/180));
		if(Axis3Node)
			Axis3Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis3.getValue()*(M_PI/180));
		if(Axis4Node)
			Axis4Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis4.getValue()*(M_PI/180));
		if(Axis5Node)
			Axis5Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis5.getValue()*(M_PI/180));
		if(Axis6Node)
			Axis6Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis6.getValue()*(M_PI/180));
    }else if (prop == &robObj->Axis1) {
        if(Axis1Node){
			Axis1Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis1.getValue()*(M_PI/180));
            if(toolShape)
                toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
        }
    }else if (prop == &robObj->Axis2) {
        if(Axis2Node){
			Axis2Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis2.getValue()*(M_PI/180));
            if(toolShape)
                toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
        }
    }else if (prop == &robObj->Axis3) {
        if(Axis3Node){
			Axis3Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis3.getValue()*(M_PI/180));
            if(toolShape)
                toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
        }
    }else if (prop == &robObj->Axis4) {
        if(Axis4Node){
			Axis4Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis4.getValue()*(M_PI/180));
            if(toolShape)
                toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
        }
    }else if (prop == &robObj->Axis5) {
        if(Axis5Node){
			Axis5Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis5.getValue()*(M_PI/180));
            if(toolShape)
                toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
        }
    }else if (prop == &robObj->Axis6) {
        if(Axis6Node){
			Axis6Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis6.getValue()*(M_PI/180));
            if(toolShape)
                toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
        }
	}else if (prop == &robObj->Tcp) {
        Base::Placement loc = robObj->Tcp.getValue();
        SbMatrix  M;
        M.setTransform(SbVec3f(loc.getPosition().x,loc.getPosition().y,loc.getPosition().z),
                       SbRotation(loc.getRotation()[0],loc.getRotation()[1],loc.getRotation()[2],loc.getRotation()[3]),
                       SbVec3f(150,150,150)
                       );
        if(pcDragger)
            pcDragger->setMotionMatrix(M);
        if(toolShape)
            toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
		//pcTcpTransform->translation = SbVec3f(loc.getPosition().x,loc.getPosition().y,loc.getPosition().z);
		//pcTcpTransform->rotation = SbRotation(loc.getRotation()[0],loc.getRotation()[1],loc.getRotation()[2],loc.getRotation()[3]);
	}else if (prop == &robObj->ToolShape) {
        App::DocumentObject* o = robObj->ToolShape.getValue<App::DocumentObject*>();

        if(o && (o->isDerivedFrom(Part::Feature::getClassTypeId()) || o->isDerivedFrom(App::VRMLObject::getClassTypeId())) ){
            //Part::Feature *p = dynamic_cast<Part::Feature *>(o);
            toolShape = Gui::Application::Instance->getViewProvider(o);
            toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
        }else
            toolShape = 0;
 	}

}
Esempio n. 5
0
File: Scene.cpp Progetto: Serge45/rl
		void
		Scene::load(const ::std::string& filename, const bool& doBoundingBoxPoints, const bool& doPoints)
		{
			::rl::xml::DomParser parser;
			
			::rl::xml::Document doc = parser.readFile(filename, "", XML_PARSE_NOENT | XML_PARSE_XINCLUDE);
			
			doc.substitute(XML_PARSE_NOENT | XML_PARSE_XINCLUDE);
			
			::rl::xml::Path path(doc);
			
			::rl::xml::Object scenes = path.eval("//scene");
			
			for (int i = 0; i < ::std::min(1, scenes.getNodeNr()); ++i)
			{
				SoInput input;
				
				if (!input.openFile(scenes.getNodeTab(i).getLocalPath(scenes.getNodeTab(i).getAttribute("href").getValue()).c_str() ,true))
				{
					throw Exception("::rl::sg::Scene::load() - failed to open file");
				}
				
				SoVRMLGroup* root = SoDB::readAllVRML(&input);
				
				if (NULL == root)
				{
					throw Exception("::rl::sg::Scene::load() - failed to read file");
				}
				
				SbViewportRegion viewportRegion;
				
				root->ref();
				
				// model
				
				::rl::xml::Object models = path.eval("model", scenes.getNodeTab(i));
				
				for (int j = 0; j < models.getNodeNr(); ++j)
				{
					SoSearchAction modelSearchAction;
					modelSearchAction.setName(models.getNodeTab(j).getAttribute("name").getValue().c_str());
					modelSearchAction.apply(root);
					
					if (NULL == modelSearchAction.getPath())
					{
						continue;
					}
					
					Model* model = this->create();
					
					model->setName(models.getNodeTab(j).getAttribute("name").getValue());
					
					// body
					
					::rl::xml::Object bodies = path.eval("body", models.getNodeTab(j));
					
					for (int k = 0; k < bodies.getNodeNr(); ++k)
					{
						SoSearchAction bodySearchAction;
						bodySearchAction.setName(bodies.getNodeTab(k).getAttribute("name").getValue().c_str());
						bodySearchAction.apply(static_cast< SoFullPath* >(modelSearchAction.getPath())->getTail());
						
						if (NULL == bodySearchAction.getPath())
						{
							continue;
						}
						
						Body* body = model->create();
						
						body->setName(bodies.getNodeTab(k).getAttribute("name").getValue());
						
						SoSearchAction pathSearchAction;
						pathSearchAction.setNode(static_cast< SoFullPath* >(bodySearchAction.getPath())->getTail());
						pathSearchAction.apply(root);
						
						SoGetMatrixAction bodyGetMatrixAction(viewportRegion);
						bodyGetMatrixAction.apply(static_cast< SoFullPath* >(pathSearchAction.getPath()));
						SbMatrix bodyMatrix = bodyGetMatrixAction.getMatrix();
						
						if (!this->isScalingSupported)
						{
							SbVec3f bodyTranslation;
							SbRotation bodyRotation;
							SbVec3f bodyScaleFactor;
							SbRotation bodyScaleOrientation;
							SbVec3f bodyCenter;
							bodyMatrix.getTransform(bodyTranslation, bodyRotation, bodyScaleFactor, bodyScaleOrientation, bodyCenter);
							
							for (int l = 0; l < 3; ++l)
							{
								if (::std::abs(bodyScaleFactor[l] - 1.0f) > 1.0e-6f)
								{
									throw Exception("::rl::sg::Scene::load() - bodyScaleFactor not supported");
								}
							}
						}
						
						::rl::math::Transform frame;
						
						for (int m = 0; m < 4; ++m)
						{
							for (int n = 0; n < 4; ++n)
							{
								frame(m, n) = bodyMatrix[n][m];
							}
						}
						
						body->setFrame(frame);
						
						if (static_cast< SoFullPath* >(bodySearchAction.getPath())->getTail()->isOfType(SoVRMLTransform::getClassTypeId()))
						{
							SoVRMLTransform* bodyVrmlTransform = static_cast< SoVRMLTransform* >(static_cast< SoFullPath* >(bodySearchAction.getPath())->getTail());
							
							for (int l = 0; l < 3; ++l)
							{
								body->center(l) = bodyVrmlTransform->center.getValue()[l];
							}
						}
						
						SoPathList pathList;
						
						// shape
						
						SoSearchAction shapeSearchAction;
						shapeSearchAction.setInterest(SoSearchAction::ALL);
						shapeSearchAction.setType(SoVRMLShape::getClassTypeId());
						shapeSearchAction.apply(static_cast< SoFullPath* >(bodySearchAction.getPath())->getTail());
						
						for (int l = 0; l < shapeSearchAction.getPaths().getLength(); ++l)
						{
							SoFullPath* path = static_cast< SoFullPath* >(shapeSearchAction.getPaths()[l]);
							
							if (path->getLength() > 1)
							{
								path = static_cast< SoFullPath* >(shapeSearchAction.getPaths()[l]->copy(1, static_cast< SoFullPath* >(shapeSearchAction.getPaths()[l])->getLength() - 1));
							}
							
							pathList.append(path);
							
							SoGetMatrixAction shapeGetMatrixAction(viewportRegion);
							shapeGetMatrixAction.apply(path);
							SbMatrix shapeMatrix = shapeGetMatrixAction.getMatrix();
							
							if (!this->isScalingSupported)
							{
								SbVec3f shapeTranslation;
								SbRotation shapeRotation;
								SbVec3f shapeScaleFactor;
								SbRotation shapeScaleOrientation;
								SbVec3f shapeCenter;
								shapeMatrix.getTransform(shapeTranslation, shapeRotation, shapeScaleFactor, shapeScaleOrientation, shapeCenter);
								
								for (int m = 0; m < 3; ++m)
								{
									if (::std::abs(shapeScaleFactor[m] - 1.0f) > 1.0e-6f)
									{
										throw Exception("::rl::sg::Scene::load() - shapeScaleFactor not supported");
									}
								}
							}
							
							SoVRMLShape* shapeVrmlShape = static_cast< SoVRMLShape* >(static_cast< SoFullPath* >(shapeSearchAction.getPaths()[l])->getTail());
							
							Shape* shape = body->create(shapeVrmlShape);
							
							shape->setName(shapeVrmlShape->getName().getString());
							
							::rl::math::Transform transform;
							
							for (int m = 0; m < 4; ++m)
							{
								for (int n = 0; n < 4; ++n)
								{
									transform(m, n) = shapeMatrix[n][m];
								}
							}
							
							shape->setTransform(transform);
						}
						
						// bounding box
						
						if (doBoundingBoxPoints)
						{
							SoGetBoundingBoxAction getBoundingBoxAction(viewportRegion);
							getBoundingBoxAction.apply(pathList);
							SbBox3f boundingBox = getBoundingBoxAction.getBoundingBox();
							
							for (int l = 0; l < 3; ++l)
							{
								body->max(l) = boundingBox.getMax()[l];
								body->min(l) = boundingBox.getMin()[l];
							}
						}
						
						// convex hull
						
						if (doPoints)
						{
							SoCallbackAction callbackAction;
							callbackAction.addTriangleCallback(SoVRMLGeometry::getClassTypeId(), Scene::triangleCallback, &body->points);
							callbackAction.apply(pathList);
						}
					}
				}
				
				root->unref();
			}
		}