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);
}
bool ViewProviderDragger::setEdit(int ModNum)
{
Q_UNUSED(ModNum);
App::DocumentObject *genericObject = this->getObject();
if (genericObject->isDerivedFrom(App::GeoFeature::getClassTypeId()))
{
App::GeoFeature *geoFeature = static_cast<App::GeoFeature *>(genericObject);
const Base::Placement &placement = geoFeature->Placement.getValue();
SoTransform *tempTransform = new SoTransform();
tempTransform->ref();
updateTransform(placement, tempTransform);
assert(!csysDragger);
csysDragger = new SoFCCSysDragger();
csysDragger->draggerSize.setValue(0.05f);
csysDragger->translation.setValue(tempTransform->translation.getValue());
csysDragger->rotation.setValue(tempTransform->rotation.getValue());
tempTransform->unref();
pcTransform->translation.connectFrom(&csysDragger->translation);
pcTransform->rotation.connectFrom(&csysDragger->rotation);
csysDragger->addStartCallback(dragStartCallback, this);
csysDragger->addFinishCallback(dragFinishCallback, this);
pcRoot->insertChild(csysDragger, 0);
TaskCSysDragger *task = new TaskCSysDragger(this, csysDragger);
Gui::Control().showDialog(task);
}
return true;
}
/*! Updates the geometry of the connectors between insertion points, which
need to move together with the robot's links. However, some of them
depend on two links, so we can not use link transforms directly (like
we do for the geometry of the insertion points).Rather, this geometry
needs to be recomputed after every change in link status.
*/
void Tendon::updateGeometry()
{
SbVec3f pPrev,pCur;
position tmpPos;
vec3 dPrev,dRes,c;
float m;
SoTransform *tran;
SoCylinder *geom;
SoTransform *neg90x = new SoTransform();
neg90x->rotation.setValue(SbVec3f(1.0f,0.0f,0.0f),-1.5707f);
/*first we wrap tendon around wrappers*/
checkWrapperIntersections();
/*and we remove unnecessary ones*/
removeWrapperIntersections();
/* we also compute the length of the tendon as the sum of connector lengths*/
currentLength = 0;
std::list<TendonInsertionPoint*>::iterator insPt, prevInsPt, newInsPt;
for (insPt=insPointList.begin(); insPt!=insPointList.end(); insPt++)
{
prevInsPt = insPt;
if (insPt!=insPointList.begin())
{
prevInsPt--;
pPrev = (*prevInsPt)->getWorldPosition();
pCur = (*insPt)->getWorldPosition();
dPrev = vec3(pCur) - vec3(pPrev);
/* distance between the two */
m = dPrev.len();
/* add it to tendon length */
currentLength += m;
/* midpoint between the two */
c = vec3(pPrev) + dPrev*0.5;
tran = (*insPt)->getIVConnectorTran();
geom = (*insPt)->getIVConnectorGeom();
geom->radius=(float)0.8;
geom->height = m;
tran->pointAt(c.toSbVec3f(),pPrev);
tran->combineLeft(neg90x);
}
}
/*after geometry has been updated, go ahead and update forces*/
computeSimplePassiveForces();
updateInsertionForces();
}
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);
}
int main(int argc, char **argv)
{
using namespace std;
int numObjects;
long nRows, nCols;
char **error;
if (argc < 4)
{
fprintf(stderr,"Usage: lab1 <input file> <output file> <rows>\n");
return -1;
}
filename = (char*)malloc(sizeof(char)*strlen(argv[2]));
strcpy(filename, argv[2]);
error = (char**)malloc(sizeof(char**)*10);
nRows = strtol(argv[3],error,10);
if (**error)
{
fprintf(stderr,"Error! use a number for rows!\n");
return -2;
}
free(error);
SoDB::init();
OSUInventorScene *scene = new OSUInventorScene(argv[1]);
//check to see if there are objects
if (numObjects = scene->Objects.getLength() < 1)
{
fprintf(stderr,"Error, no objects");
return -2;
}
//get the objects and put them into a list
for (int i = 0; i < scene->Objects.getLength(); i++)
{
OSUObjectData *obj = (OSUObjectData *)scene->Objects[i];
/*
if (!obj->Check())
{
fprintf(stderr,"Error detected in OSUObjectData for object %i.\n",i);
return 20;
}
*/
SoType shape_type = obj->shape->getTypeId();
if (shape_type == SoSphere::getClassTypeId())
{
Sphere *sp = new Sphere;
SoSphere * sphere = (SoSphere *) obj->shape;
SoTransform * transformation = obj->transformation;
SbVec3f scale_vector = transformation->scaleFactor.getValue();
SbVec3f translation_vector = transformation->translation.getValue();
sp->radius = 1;
sp->center = Point(0,0,0,1);
//do the materials stuff
SoMaterial * material = obj->material;
sp->color = rgb(material->diffuseColor[0][0],material->diffuseColor[0][1],material->diffuseColor[0][2]);
sp->specular = rgb(material->specularColor[0][0], material->specularColor[0][1],material->specularColor[0][2]);
sp->ambient = rgb(material->ambientColor[0][0], material->ambientColor[0][1], material->ambientColor[0][2]);
sp->shininess = material->shininess[0];
sp->trans = material->transparency[0];
printf("Object %d has shininess %f\n", sp->object_number, sp->shininess);
//let's get our translation space matrix
//void getTranslationSpaceMatrix(SbMatrix &mat, SbMatrix &inv) const
SbMatrix tmat, tinv, smat, sinv, rmat, rinv;
transformation->getTranslationSpaceMatrix(tmat, tinv);
// transformation->getScaleSpaceMatrix(smat,sinv);
transformation->getRotationSpaceMatrix(rmat, rinv);
// SbMatrix & multRight(const SbMatrix &m)
sp->M = tmat;
sp->Mi = tinv;
sp->M.transpose();
sp->Mi.transpose();
sp->type = eSPHERE;
//texture data
SoSFImage image;
int nc;
SbVec2s size;
const unsigned char *texelArray;
// get properties of object
//obj = (OSUObjectData *)worldptr->scene->Objects[i];
if (obj->texture != NULL)
{
image = obj->texture->image;
texelArray = image.getValue(size,nc); // sets 'size', 'nc', 'texelArray'
sp->image = texelArray[0];
printf("\nimage value = %d\n",texelArray[0]);
}
objects.push_back(sp);
sp->object_number = i;
}
if (shape_type == SoCube::getClassTypeId())
{
Box *sp = new Box;
SoSphere * sphere = (SoSphere *) obj->shape;
SoTransform * transformation = obj->transformation;
SbVec3f scale_vector = transformation->scaleFactor.getValue();
SbVec3f translation_vector = transformation->translation.getValue();
sp->center = Point(0,0,0,1);
//do the materials stuff
SoMaterial * material = obj->material;
sp->color = rgb(material->diffuseColor[0][0],material->diffuseColor[0][1],material->diffuseColor[0][2]);
sp->specular = rgb(material->specularColor[0][0], material->specularColor[0][1],material->specularColor[0][2]);
sp->ambient = rgb(material->ambientColor[0][0], material->ambientColor[0][1], material->ambientColor[0][2]);
sp->shininess = material->shininess[0];
sp->trans = material->transparency[0];
printf("Object %d has shininess %f\n", sp->object_number, sp->shininess);
//let's get our translation space matrix
//void getTranslationSpaceMatrix(SbMatrix &mat, SbMatrix &inv) const
SbMatrix tmat, tinv, smat, sinv, rmat, rinv;
transformation->getTranslationSpaceMatrix(tmat, tinv);
// transformation->getScaleSpaceMatrix(smat,sinv);
transformation->getRotationSpaceMatrix(rmat, rinv);
// list triangles
// SbMatrix & multRight(const SbMatrix &m)
sp->M = tmat;
sp->Mi = tinv;
sp->M.transpose();
sp->Mi.transpose();
SoSFImage image;
int nc;
SbVec2s size;
const unsigned char *texelArray;
// get properties of object
//obj = (OSUObjectData *)worldptr->scene->Objects[i];
if (obj->texture != NULL)
{
image = obj->texture->image;
texelArray = image.getValue(size,nc); // sets 'size' // list triangles
printf("\nimage value = %d\n",texelArray[0]);
sp->image = texelArray[0];
}
objects.push_back(sp);
sp->type = eCUBE;
sp->object_number = i;
}
if (shape_type == SoIndexedTriangleStripSet::getClassTypeId())
{
SoIndexedTriangleStripSet * triangle_strip_set =
(SoIndexedTriangleStripSet *) obj->shape;
if (obj->points == NULL)
{
cout << " Error: Points missing for indexed triangle strip set.";
}
else if (obj->points->getTypeId() != SoCoordinate3::getClassTypeId())
{
cout << " Error: Incorrect format for point list." << endl;
cout << " Expected list of 3D coordinates.";
}
else
{
SoCoordinate3 * coord = (SoCoordinate3 *) obj->points;
int numv = coord->point.getNum();
cout << " Surface has " << numv << " vertices." << endl;
for (int i = 0; i < numv; i++)
{
cout << " Vertex " << i << " = ("
<< coord->point[i][0] << ", "
<< coord->point[i][1] << ", "
<< coord->point[i][2] << ")." << endl;
}
if (obj->normals == NULL)
{
cout << " No stored normals." << endl;
}
else
{
int num_normals = obj->normals->vector.getNum();
if (num_normals != numv)
{
// assume PER_VERTEX normal binding
cout << "Error: Number of normals does not equal number of vertices." << endl;
}
else
{
for (int i = 0; i < num_normals; i++) {
cout << " Normal " << i << " = ("
<< obj->normals->vector[i][0] << ", "
<< obj->normals->vector[i][1] << ", "
<< obj->normals->vector[i][2] << ")." << endl;
}
}
}
/*
// list triangles
int itriangle = 0;
int icoord = 0;
int coord_list_length = triangle_strip_set->coordIndex.getNum();
while (icoord < coord_list_length) {
int c0 = SO_END_STRIP_INDEX;
int c1 = SO_END_STRIP_INDEX;
int c2 = SO_END_STRIP_INDEX;
while (icoord < coord_list_length &&
triangle_strip_set->coordIndex[icoord] != SO_END_STRIP_INDEX) { c2 = triangle_strip_set->coordIndex[icoord];
if (c0 != SO_END_STRIP_INDEX && c1 != SO_END_STRIP_INDEX) {
cout << " Triangle " << itriangle
<< " coordinate indices = ( "
<< c0 << ", " << c1 << ", " << c2 << " )." << endl;
itriangle++;
};
icoord++;
c0 = c1;
c1 = c2;
}
}
*/
}
}
free(obj);
}
printf("Number of objects seen %d\n", objects.size());
printf("Setting up camera\n");
/*********************************************************************
* Camera Setup
*
*********************************************************************/
SoCamera * cam = scene->Camera;
if (scene->Camera == NULL)
{
printf("No camera found. Setting Default/n");
camera.aspect = 1;
camera.position = Point(0,0,0,1);
camera.n = Vector(0,0,-1,0);
camera.v = Vector(0,1,0,0);
camera.u = cross(camera.n,camera.v);
camera.height_angle = M_PI/2;
}
else
{
SbVec3f camera_position = cam->position.getValue();
SbRotation cam_orientation = cam->orientation.getValue();
SbVec3f camera_rotation_axis;
float cam_rotation_angle;
cam_orientation.getValue(camera_rotation_axis, cam_rotation_angle);
float cam_aspect_ratio = cam->aspectRatio.getValue();
SoType cam_type = cam->getTypeId();
//Let's grab the aspect ratio
nCols =(int)( nRows*cam_aspect_ratio);
camera.aspect = cam_aspect_ratio;
printf("Number of rows is %d columns is %d with an aspect ratio of %f\n",nRows,nCols,cam_aspect_ratio);
// calculate camera direction and camera up direction
SbVec3f camera_direction, camera_up;
cam_orientation.multVec(SbVec3f(0, 0, -1), camera_direction);
cam_orientation.multVec(SbVec3f(0, 1, 0), camera_up);
camera.n = -1*Vector(camera_direction[0],camera_direction[1],camera_direction[2],0);
camera.v = -1*Vector(camera_up[0],camera_up[1],camera_up[2],0);
camera.position = Point(camera_position[0],camera_position[1],camera_position[2],1);
camera.u = cross(camera.n,camera.v);
if (cam_type == SoPerspectiveCamera::getClassTypeId()) {
// camera is a perspective camera
SoPerspectiveCamera * perspective_camera = (SoPerspectiveCamera *) cam;
float camera_height_angle = perspective_camera->heightAngle.getValue();
camera.height_angle = camera_height_angle;
}
}
N = 1;
pixH = 2*tan(camera.height_angle/2)/nCols;
pixW = 2*tan(camera.height_angle/2)/nRows;
printf("Camera position is %f %f %f\n",camera.position.x,camera.position.y, camera.position.z);
printf("Camera n is %f %f %f\n",camera.n.x,camera.n.y, camera.n.z);
printf("Camera v is %f %f %f\n",camera.v.x,camera.v.y, camera.v.z);
printf("Camera u is %f %f %f\n",camera.u.x,camera.u.y, camera.u.z);
printf("N is %f\n",N);
//setup for lights
// list lights
for (int j = 0; j < scene->Lights.getLength(); j++) {
SoLight * flight = (SoLight *) scene->Lights[j];
SoType light_type = flight->getTypeId();
SoSFColor lightColor;
SbColor lightClr;
float lightr,lightg,lightb;
lightColor = flight->color;
light *l = new light;
l->color = rgb(lightClr[0],lightClr[1],lightClr[2]);
l->intensity = flight->intensity.getValue();
if (light_type == SoPointLight::getClassTypeId())
{
SoPointLight * point_light = (SoPointLight *) flight;
SbVec3f location = point_light->location.getValue();
l->center = Point(location[0],location[1],location[2],1);
l->type = ePOINT;
}
else if (light_type == SoSpotLight::getClassTypeId())
{
SoSpotLight * spot_light = (SoSpotLight *) flight;
SbVec3f location = spot_light->location.getValue();
l->center = Point(location[0], location[1], location[2],1);
l->theta = spot_light->cutOffAngle.getValue();
l->drop_off = spot_light->dropOffRate.getValue();
SbVec3f direction = spot_light->direction.getValue();
Vector lightDir(direction[0], direction[1], direction[2],0);
l->dir = lightDir;
l->type=eHOOD;
}
else if (light_type == SoDirectionalLight::getClassTypeId())
{
l->type = eDIR;
SoDirectionalLight * dir_light = (SoDirectionalLight *) flight;
SbVec3f direction = dir_light->direction.getValue();
Vector lightDir(direction[0], direction[1], direction[2],0);
l->dir = lightDir;
}
lights.push_back(l);
}
//start our tracer
trace(objects, nRows, nCols, 1);
//let's free our vector pointers
int x = objects.size() - 1;
fprintf(stderr,"Freeing objects\n");
while (x >= 0)
{
delete objects[x];
// printf("freeing object %d\n",x);
x--;
}
x = lights.size() - 1;
fprintf(stderr,"Freeing lights\n");
while (x >= 0)
{
delete lights[x];
x--;
}
}
/*! 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);
*/
}
static SoSeparator *
setUpGraph(const SbViewportRegion &vpReg,
SoInput *sceneInput,
Options &options)
//
//////////////////////////////////////////////////////////////
{
// Create a root separator to hold everything. Turn
// caching off, since the transformation will blow
// it anyway.
SoSeparator *root = new SoSeparator;
root->ref();
root->renderCaching = SoSeparator::OFF;
// Add a camera to view the scene
SoPerspectiveCamera *camera = new SoPerspectiveCamera;
root->addChild(camera);
// Add a transform node to spin the scene
SoTransform *sceneTransform = new SoTransform;
sceneTransform->setName(SCENE_XFORM_NAME);
root->addChild(sceneTransform);
// Read and add input scene graph
SoSeparator *inputRoot = SoDB::readAll(sceneInput);
if (inputRoot == NULL) {
fprintf(stderr, "Cannot read scene graph\n");
root->unref();
exit(1);
}
root->addChild(inputRoot);
SoPath *path;
SoGroup *parent, *group;
SoSearchAction act;
// expand out all File nodes and replace them with groups
// containing the children
SoFile *fileNode;
act.setType(SoFile::getClassTypeId());
act.setInterest(SoSearchAction::FIRST);
act.apply(inputRoot);
while ((path = act.getPath()) != NULL) {
fileNode = (SoFile *) path->getTail();
path->pop();
parent = (SoGroup *) path->getTail();
group = fileNode->copyChildren();
if (group) {
parent->replaceChild(fileNode, group);
// apply action again and continue
act.apply(inputRoot);
}
}
// expand out all node kits and replace them with groups
// containing the children
SoBaseKit *kitNode;
SoChildList *childList;
act.setType(SoBaseKit::getClassTypeId());
act.setInterest(SoSearchAction::FIRST);
act.apply(inputRoot);
while ((path = act.getPath()) != NULL) {
kitNode = (SoBaseKit *) path->getTail();
path->pop();
parent = (SoGroup *) path->getTail();
group = new SoGroup;
childList = kitNode->getChildren();
for (int i=0; i<childList->getLength(); i++)
group->addChild((*childList)[i]);
parent->replaceChild(kitNode, group);
act.apply(inputRoot);
}
// check to see if there are any lights
// if no lights, add a directional light to the scene
act.setType(SoLight::getClassTypeId());
act.setInterest(SoSearchAction::FIRST);
act.apply(inputRoot);
if (act.getPath() == NULL) { // no lights
SoDirectionalLight *light = new SoDirectionalLight;
root->insertChild(light, 1);
}
else
options.hasLights = TRUE;
// check to see if there are any texures in the scene
act.setType(SoTexture2::getClassTypeId());
act.setInterest(SoSearchAction::FIRST);
act.apply(inputRoot);
if (act.getPath() != NULL)
options.hasTextures = TRUE;
camera->viewAll(root, vpReg);
// print out information about the scene graph
int32_t numTris, numLines, numPoints, numNodes;
countPrimitives( inputRoot, numTris, numLines, numPoints, numNodes );
printf("Number of nodes in scene graph: %d\n", numNodes );
printf("Number of triangles in scene graph: %d\n", numTris );
printf("Number of lines in scene graph: %d\n", numLines );
printf("Number of points in scene graph: %d\n\n", numPoints );
// Make the center of rotation the center of
// the scene
SoGetBoundingBoxAction bba(vpReg);
bba.apply(root);
sceneTransform->center = bba.getBoundingBox().getCenter();
return root;
}
