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