SoPickedPoint* ViewProvider::getPointOnRay(const SbVec2s& pos, const View3DInventorViewer* viewer) const { //first get the path to this node and calculate the current transformation SoSearchAction sa; sa.setNode(pcRoot); sa.setSearchingAll(true); sa.apply(viewer->getSoRenderManager()->getSceneGraph()); if (!sa.getPath()) return nullptr; SoGetMatrixAction gm(viewer->getSoRenderManager()->getViewportRegion()); gm.apply(sa.getPath()); SoTransform* trans = new SoTransform; trans->setMatrix(gm.getMatrix()); trans->ref(); // build a temporary scenegraph only keeping this viewproviders nodes and the accumulated // transformation SoSeparator* root = new SoSeparator; root->ref(); root->addChild(viewer->getSoRenderManager()->getCamera()); root->addChild(trans); root->addChild(pcRoot); //get the picked point SoRayPickAction rp(viewer->getSoRenderManager()->getViewportRegion()); rp.setPoint(pos); rp.setRadius(viewer->getPickRadius()); rp.apply(root); root->unref(); trans->unref(); SoPickedPoint* pick = rp.getPickedPoint(); return (pick ? new SoPickedPoint(*pick) : 0); }
SoSeparator * newSphere(double x, double y, double z) { SbMatrix tr = SbMatrix::identity(); tr.setTranslate(SbVec3f(x,y,z)); SoTransform * t = new SoTransform; t->setMatrix(tr); SoSeparator * sphere_sep = new SoSeparator; SoMaterial * black = new SoMaterial; SoMFColor black_color; black_color.setValue(0.0,0.0,0.0); black->ambientColor = black_color; black->specularColor = black_color; black->emissiveColor = black_color; black->transparency = 0.8f; sphere_sep->addChild(black); sphere_sep->addChild(t); SoSphere * sphere = new SoSphere; sphere->radius = 10; sphere_sep->addChild(sphere); return sphere_sep; }
SoPickedPoint* ViewProvider::getPointOnRay(const SbVec3f& pos,const SbVec3f& dir, const View3DInventorViewer* viewer) const { // Note: There seems to be a bug with setRay() which causes SoRayPickAction // to fail to get intersections between the ray and a line //first get the path to this node and calculate the current setTransformation SoSearchAction sa; sa.setNode(pcRoot); sa.setSearchingAll(true); sa.apply(viewer->getSoRenderManager()->getSceneGraph()); SoGetMatrixAction gm(viewer->getSoRenderManager()->getViewportRegion()); gm.apply(sa.getPath()); // build a temporary scenegraph only keeping this viewproviders nodes and the accumulated // transformation SoTransform* trans = new SoTransform; trans->ref(); trans->setMatrix(gm.getMatrix()); SoSeparator* root = new SoSeparator; root->ref(); root->addChild(viewer->getSoRenderManager()->getCamera()); root->addChild(trans); root->addChild(pcRoot); //get the picked point SoRayPickAction rp(viewer->getSoRenderManager()->getViewportRegion()); rp.setRay(pos,dir); rp.setRadius(viewer->getPickRadius()); rp.apply(root); root->unref(); trans->unref(); // returns a copy of the point SoPickedPoint* pick = rp.getPickedPoint(); //return (pick ? pick->copy() : 0); // needs the same instance of CRT under MS Windows return (pick ? new SoPickedPoint(*pick) : 0); }
/*! Gives us a visual indicator of what this contact looks like, in WORLD COORDINATES. Since this contact has to be transformed to world coordinates for the sake of grasp analysis, this allows us to be sure that the transformation makes sense. It assumes WRENCHES have been computed. */ void VirtualContact::getWorldIndicator(bool useObjectData) { vec3 forceVec; position worldLoc; if (!useObjectData) { worldLoc = getWorldLocation(); } else { vec3 objDist; getObjectDistanceAndNormal(body2, &objDist, NULL); worldLoc = getWorldLocation() + objDist; } SoTransform* tran = new SoTransform; SbMatrix tr; tr.setTranslate( worldLoc.toSbVec3f() ); tran->setMatrix( tr ); SbVec3f *points = (SbVec3f*)calloc(numFCWrenches+1, sizeof(SbVec3f) ); int32_t *cIndex = (int32_t*)calloc(4*numFCWrenches, sizeof(int32_t) ); points[0].setValue(0,0,0); for (int i=0;i<numFCWrenches;i++) { //if ( wrench[i].torque.len() != 0 ) continue; forceVec = wrench[i].force; forceVec = Body::CONE_HEIGHT * forceVec; points[i+1].setValue( forceVec.x(), forceVec.y(), forceVec.z() ); cIndex[4*i] = 0; cIndex[4*i+1] = i + 2; if ( i == numFCWrenches-1) cIndex[4*i+1] = 1; cIndex[4*i+2] = i + 1; cIndex[4*i+3] = -1; } SoCoordinate3* coords = new SoCoordinate3; SoIndexedFaceSet* ifs = new SoIndexedFaceSet; coords->point.setValues(0,numFCWrenches+1,points); ifs->coordIndex.setValues(0,4*numFCWrenches,cIndex); free(points); free(cIndex); SoMaterial *coneMat = new SoMaterial; coneMat->diffuseColor = SbColor(0.0f,0.0f,0.8f); coneMat->ambientColor = SbColor(0.0f,0.0f,0.2f); coneMat->emissiveColor = SbColor(0.0f,0.0f,0.4f); if (mWorldInd) { body1->getWorld()->getIVRoot()->removeChild(mWorldInd); } mWorldInd = new SoSeparator; mWorldInd->addChild(tran); mWorldInd->addChild(coneMat); mWorldInd->addChild(coords); mWorldInd->addChild(ifs); body1->getWorld()->getIVRoot()->addChild(mWorldInd); /* SoSeparator* cSep = new SoSeparator; tr.setTranslate( mCenter.toSbVec3f() ); tran = new SoTransform; tran->setMatrix( tr ); cSep->addChild(tran); SoSphere* cSphere = new SoSphere(); cSphere->radius = 5; cSep->addChild(cSphere); body1->getWorld()->getIVRoot()->addChild(cSep); */ }