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);
*/
}
