00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include "PreCompiled.h"
00025
00026 #ifndef _PreComp_
00027 # include <Inventor/SoDB.h>
00028 # include <Inventor/SoInput.h>
00029 # include <Inventor/SbVec3f.h>
00030 # include <Inventor/nodes/SoSeparator.h>
00031 # include <Inventor/nodes/SoTransform.h>
00032 # include <Inventor/nodes/SoSphere.h>
00033 # include <Inventor/nodes/SoRotation.h>
00034 # include <Inventor/actions/SoSearchAction.h>
00035 # include <Inventor/draggers/SoJackDragger.h>
00036 # include <Inventor/draggers/SoTrackballDragger.h>
00037 # include <Inventor/VRMLnodes/SoVRMLTransform.h>
00038 # include <QFile>
00039 #endif
00040
00041 #include "ViewProviderRobotObject.h"
00042
00043 #include <Mod/Robot/App/RobotObject.h>
00044 #include <Mod/Part/App/PartFeature.h>
00045 #include <Mod/Part/Gui/ViewProvider.h>
00046 #include <App/Document.h>
00047 #include <App/VRMLObject.h>
00048 #include <Gui/Application.h>
00049 #include <Base/FileInfo.h>
00050 #include <Base/Stream.h>
00051 #include <Base/Console.h>
00052 #include <sstream>
00053 using namespace Gui;
00054 using namespace RobotGui;
00055
00056 PROPERTY_SOURCE(RobotGui::ViewProviderRobotObject, Gui::ViewProviderGeometryObject)
00057
00058 ViewProviderRobotObject::ViewProviderRobotObject()
00059 : pcDragger(0),toolShape(0)
00060 {
00061 ADD_PROPERTY(Manipulator,(0));
00062
00063 pcRobotRoot = new Gui::SoFCSelection();
00064 pcRobotRoot->highlightMode = Gui::SoFCSelection::OFF;
00065
00066
00067 pcRobotRoot->ref();
00068
00069 pcSimpleRoot = new Gui::SoFCSelection();
00070 pcSimpleRoot->highlightMode = Gui::SoFCSelection::OFF;
00071
00072 pcSimpleRoot->ref();
00073
00074 pcOffRoot = new SoGroup();
00075 pcOffRoot->ref();
00076
00077
00078 pcTcpRoot = new SoGroup();
00079 pcTcpRoot->ref();
00080
00081
00082 Axis1Node = Axis2Node = Axis3Node = Axis4Node = Axis5Node = Axis6Node = 0;
00083 }
00084
00085 void ViewProviderRobotObject::setDragger()
00086 {
00087 assert(pcDragger==0);
00088 pcDragger = new SoJackDragger();
00089 pcDragger->addMotionCallback(sDraggerMotionCallback,this);
00090 pcTcpRoot->addChild(pcDragger);
00091
00092
00093 Robot::RobotObject* robObj = static_cast<Robot::RobotObject*>(pcObject);
00094 Base::Placement loc = robObj->Tcp.getValue();
00095 SbMatrix M;
00096 M.setTransform(SbVec3f(loc.getPosition().x,loc.getPosition().y,loc.getPosition().z),
00097 SbRotation(loc.getRotation()[0],loc.getRotation()[1],loc.getRotation()[2],loc.getRotation()[3]),
00098 SbVec3f(150,150,150)
00099 );
00100 pcDragger->setMotionMatrix(M);
00101
00102
00103 }
00104 void ViewProviderRobotObject::resetDragger()
00105 {
00106 assert(pcDragger);
00107 pcTcpRoot->removeAllChildren();
00108 pcDragger = 0;
00109
00110 }
00111
00112 ViewProviderRobotObject::~ViewProviderRobotObject()
00113 {
00114 pcRobotRoot->unref();
00115 pcSimpleRoot->unref();
00116 pcOffRoot->unref();
00117
00118 }
00119
00120 void ViewProviderRobotObject::attach(App::DocumentObject *pcObj)
00121 {
00122 ViewProviderGeometryObject::attach(pcObj);
00123
00124 addDisplayMaskMode(pcRobotRoot, "VRML");
00125 pcRobotRoot->objectName = pcObj->getNameInDocument();
00126 pcRobotRoot->documentName = pcObj->getDocument()->getName();
00127 pcRobotRoot->subElementName = "Main";
00128 pcRobotRoot->addChild(pcTcpRoot);
00129
00130 addDisplayMaskMode(pcSimpleRoot, "Simple");
00131 pcSimpleRoot->objectName = pcObj->getNameInDocument();
00132 pcSimpleRoot->documentName = pcObj->getDocument()->getName();
00133 pcSimpleRoot->subElementName = "Main";
00134 pcSimpleRoot->addChild(pcTcpRoot);
00135
00136 addDisplayMaskMode(pcOffRoot, "Off");
00137 pcOffRoot->addChild(pcTcpRoot);
00138
00139
00140 }
00141
00142 void ViewProviderRobotObject::setDisplayMode(const char* ModeName)
00143 {
00144 if ( strcmp("VRML",ModeName)==0 )
00145 setDisplayMaskMode("VRML");
00146 if ( strcmp("Simple",ModeName)==0 )
00147 setDisplayMaskMode("Simple");
00148 if ( strcmp("Off",ModeName)==0 )
00149 setDisplayMaskMode("Off");
00150 ViewProviderGeometryObject::setDisplayMode( ModeName );
00151 }
00152
00153 std::vector<std::string> ViewProviderRobotObject::getDisplayModes(void) const
00154 {
00155 std::vector<std::string> StrList;
00156 StrList.push_back("VRML");
00157 StrList.push_back("Simple");
00158 StrList.push_back("Off");
00159 return StrList;
00160 }
00161
00162 void ViewProviderRobotObject::onChanged(const App::Property* prop)
00163 {
00164 if (prop == &Manipulator) {
00165 if (Manipulator.getValue()) {
00166 if (!this->pcDragger)
00167 this->setDragger();
00168 }
00169 else {
00170 if (this->pcDragger)
00171 this->resetDragger();
00172 }
00173 }
00174 else {
00175 ViewProviderGeometryObject::onChanged(prop);
00176 }
00177 }
00178
00179 void ViewProviderRobotObject::updateData(const App::Property* prop)
00180 {
00181 Robot::RobotObject* robObj = static_cast<Robot::RobotObject*>(pcObject);
00182 if (prop == &robObj->RobotVrmlFile) {
00183
00184 const char* filename = robObj->RobotVrmlFile.getValue();
00185 QString fn = QString::fromUtf8(filename);
00186 QFile file(fn);
00187 SoInput in;
00188 pcRobotRoot->removeAllChildren();
00189 if (!fn.isEmpty() && file.open(QFile::ReadOnly)) {
00190 QByteArray buffer = file.readAll();
00191 in.setBuffer((void *)buffer.constData(), buffer.length());
00192 SoSeparator * node = SoDB::readAll(&in);
00193 if (node) pcRobotRoot->addChild(node);
00194 pcRobotRoot->addChild(pcTcpRoot);
00195 }
00196
00197 Axis1Node = Axis2Node = Axis3Node = Axis4Node = Axis5Node = Axis6Node = 0;
00198 SoSearchAction searchAction;
00199 SoPath * path;
00200
00201
00202 searchAction.setName("FREECAD_AXIS1");
00203 searchAction.setInterest(SoSearchAction::FIRST);
00204 searchAction.setSearchingAll(FALSE);
00205 searchAction.apply(pcRobotRoot);
00206 path = searchAction.getPath();
00207 if(path){
00208 SoNode* node = path->getTail();
00209 std::string typeName = (const char*)node->getTypeId().getName();
00210 if (!node || node->getTypeId() != SoVRMLTransform::getClassTypeId())
00211 throw;
00212 Axis1Node = static_cast<SoVRMLTransform *>(node);
00213 }
00214
00215 searchAction.setName("FREECAD_AXIS2");
00216 searchAction.setInterest(SoSearchAction::FIRST);
00217 searchAction.setSearchingAll(FALSE);
00218 searchAction.apply(pcRobotRoot);
00219 path = searchAction.getPath();
00220 if(path){
00221 SoNode* node = path->getTail();
00222 std::string typeName = (const char*)node->getTypeId().getName();
00223 if (!node || node->getTypeId() != SoVRMLTransform::getClassTypeId())
00224 throw;
00225 Axis2Node = static_cast<SoVRMLTransform *>(node);
00226 }
00227
00228 searchAction.setName("FREECAD_AXIS3");
00229 searchAction.setInterest(SoSearchAction::FIRST);
00230 searchAction.setSearchingAll(FALSE);
00231 searchAction.apply(pcRobotRoot);
00232 path = searchAction.getPath();
00233 if(path){
00234 SoNode* node = path->getTail();
00235 std::string typeName = (const char*)node->getTypeId().getName();
00236 if (!node || node->getTypeId() != SoVRMLTransform::getClassTypeId())
00237 throw;
00238 Axis3Node = static_cast<SoVRMLTransform *>(node);
00239 }
00240
00241 searchAction.setName("FREECAD_AXIS4");
00242 searchAction.setInterest(SoSearchAction::FIRST);
00243 searchAction.setSearchingAll(FALSE);
00244 searchAction.apply(pcRobotRoot);
00245 path = searchAction.getPath();
00246 if(path){
00247 SoNode* node = path->getTail();
00248 std::string typeName = (const char*)node->getTypeId().getName();
00249 if (!node || node->getTypeId() != SoVRMLTransform::getClassTypeId())
00250 throw;
00251 Axis4Node = static_cast<SoVRMLTransform *>(node);
00252 }
00253
00254 searchAction.setName("FREECAD_AXIS5");
00255 searchAction.setInterest(SoSearchAction::FIRST);
00256 searchAction.setSearchingAll(FALSE);
00257 searchAction.apply(pcRobotRoot);
00258 path = searchAction.getPath();
00259 if(path){
00260 SoNode* node = path->getTail();
00261 std::string typeName = (const char*)node->getTypeId().getName();
00262 if (!node || node->getTypeId() != SoVRMLTransform::getClassTypeId())
00263 throw;
00264 Axis5Node = static_cast<SoVRMLTransform *>(node);
00265 }
00266
00267 searchAction.setName("FREECAD_AXIS6");
00268 searchAction.setInterest(SoSearchAction::FIRST);
00269 searchAction.setSearchingAll(FALSE);
00270 searchAction.apply(pcRobotRoot);
00271 path = searchAction.getPath();
00272 if(path){
00273 SoNode* node = path->getTail();
00274 std::string typeName = (const char*)node->getTypeId().getName();
00275 if (!node || node->getTypeId() != SoVRMLTransform::getClassTypeId())
00276 throw;
00277 Axis6Node = static_cast<SoVRMLTransform *>(node);
00278 }
00279 if(Axis1Node)
00280 Axis1Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis1.getValue()*(M_PI/180));
00281 if(Axis2Node)
00282 Axis2Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis2.getValue()*(M_PI/180));
00283 if(Axis3Node)
00284 Axis3Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis3.getValue()*(M_PI/180));
00285 if(Axis4Node)
00286 Axis4Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis4.getValue()*(M_PI/180));
00287 if(Axis5Node)
00288 Axis5Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis5.getValue()*(M_PI/180));
00289 if(Axis6Node)
00290 Axis6Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis6.getValue()*(M_PI/180));
00291 }else if (prop == &robObj->Axis1) {
00292 if(Axis1Node){
00293 Axis1Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis1.getValue()*(M_PI/180));
00294 if(toolShape)
00295 toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
00296 }
00297 }else if (prop == &robObj->Axis2) {
00298 if(Axis2Node){
00299 Axis2Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis2.getValue()*(M_PI/180));
00300 if(toolShape)
00301 toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
00302 }
00303 }else if (prop == &robObj->Axis3) {
00304 if(Axis3Node){
00305 Axis3Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis3.getValue()*(M_PI/180));
00306 if(toolShape)
00307 toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
00308 }
00309 }else if (prop == &robObj->Axis4) {
00310 if(Axis4Node){
00311 Axis4Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis4.getValue()*(M_PI/180));
00312 if(toolShape)
00313 toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
00314 }
00315 }else if (prop == &robObj->Axis5) {
00316 if(Axis5Node){
00317 Axis5Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis5.getValue()*(M_PI/180));
00318 if(toolShape)
00319 toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
00320 }
00321 }else if (prop == &robObj->Axis6) {
00322 if(Axis6Node){
00323 Axis6Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis6.getValue()*(M_PI/180));
00324 if(toolShape)
00325 toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
00326 }
00327 }else if (prop == &robObj->Tcp) {
00328 Base::Placement loc = robObj->Tcp.getValue();
00329 SbMatrix M;
00330 M.setTransform(SbVec3f(loc.getPosition().x,loc.getPosition().y,loc.getPosition().z),
00331 SbRotation(loc.getRotation()[0],loc.getRotation()[1],loc.getRotation()[2],loc.getRotation()[3]),
00332 SbVec3f(150,150,150)
00333 );
00334 if(pcDragger)
00335 pcDragger->setMotionMatrix(M);
00336 if(toolShape)
00337 toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
00338
00339
00340 }else if (prop == &robObj->ToolShape) {
00341 App::DocumentObject* o = robObj->ToolShape.getValue<App::DocumentObject*>();
00342
00343 if(o && (o->isDerivedFrom(Part::Feature::getClassTypeId()) || o->isDerivedFrom(App::VRMLObject::getClassTypeId())) ){
00344
00345 toolShape = Gui::Application::Instance->getViewProvider(o);
00346 toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
00347 }else
00348 toolShape = 0;
00349 }
00350
00351 }
00352 void ViewProviderRobotObject::setAxisTo(float A1,float A2,float A3,float A4,float A5,float A6,const Base::Placement &Tcp)
00353 {
00354 Robot::RobotObject* robObj = static_cast<Robot::RobotObject*>(pcObject);
00355
00356 if(Axis1Node)
00357
00358 Axis1Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),A1*(M_PI/180));
00359 if(Axis2Node)
00360 Axis2Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),A2*(M_PI/180));
00361 if(Axis3Node)
00362 Axis3Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),A3*(M_PI/180));
00363 if(Axis4Node)
00364 Axis4Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),A4*(M_PI/180));
00365 if(Axis5Node)
00366 Axis5Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),A5*(M_PI/180));
00367 if(Axis6Node)
00368 Axis6Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),A6*(M_PI/180));
00369
00370 if(toolShape)
00371 toolShape->setTransformation((Tcp * (robObj->ToolBase.getValue().inverse())).toMatrix());
00372 }
00373
00374 void ViewProviderRobotObject::sDraggerMotionCallback(void *data, SoDragger *dragger)
00375 {
00376 static_cast<ViewProviderRobotObject*>(data)->DraggerMotionCallback(dragger);
00377 }
00378
00379 void ViewProviderRobotObject::DraggerMotionCallback(SoDragger *dragger)
00380 {
00381 float q0, q1, q2, q3;
00382
00383 Robot::RobotObject* robObj = static_cast<Robot::RobotObject*>(pcObject);
00384 Base::Placement Tcp = robObj->Tcp.getValue();
00385 const SbMatrix & M = dragger->getMotionMatrix ();
00386 SbVec3f translation;
00387 SbRotation rotation;
00388 SbVec3f scaleFactor;
00389 SbRotation scaleOrientation;
00390 SbVec3f center(Tcp.getPosition().x,Tcp.getPosition().y,Tcp.getPosition().z);
00391 M.getTransform(translation, rotation, scaleFactor, scaleOrientation);
00392 rotation.getValue(q0, q1, q2, q3);
00393
00394 Base::Rotation rot(q0, q1, q2, q3);
00395 Base::Vector3d pos(translation[0],translation[1],translation[2]);
00396 robObj->Tcp.setValue(Base::Placement(pos,rot));
00397 }
00398