bool EnhancedIKSolver::solve_chain(int root_bone, int end_bone,
const float3& position, int current_frame) {
std::vector<int> indices;
fill_chain(root_bone, end_bone, current_frame, indices);
//Calculate in root_bone coordinate system where is the goal position
osg::Matrix m;
skeleton->get_node(root_bone)->get_node_world_matrix_origin(current_frame,
m);
osg::Vec3 position_osg(position.x, position.y, position.z);
osg::Vec3 goal_position = position_osg * m;
//Solve and update the rotations
if (ik_solver.solve_chain(make_float3(goal_position._v))) {
int j = 0;
for (unsigned int i = 0; i < indices.size(); i++) {
Node * node = skeleton->get_node(indices[i]);
float4 new_rot;
ik_solver.get_rotation_joint(j, new_rot);
node->quat_arr.at(current_frame).set(new_rot.x, new_rot.y,
new_rot.z, new_rot.w);
j++;
}
return true;
} else {
return false;
}
}
void
renderSceneToImage(::osg::Node* node, const ::std::string& sFileName_,double position[3],double target[3],double up[3])
{
osg::Group* root = new osg::Group();
// Declare transform, initialize with defaults.
osg::PositionAttitudeTransform* nodeXform =
new osg::PositionAttitudeTransform();
// Use the 'addChild' method of the osg::Group class to
// add the transform as a child of the root node and the
// node node as a child of the transform.
root->addChild(nodeXform);
{
Moby::CcolorVisitor newColor;
newColor.setColor(0,0,0,0);
node->accept( newColor );
}
nodeXform->addChild(node);
if(!sceneFile.empty()){
::osg::Node* sceneNode = osgDB::readNodeFile(sceneFile);
nodeXform->addChild(sceneNode);
}
// Declare and initialize a Vec3 instance to change the
// position of the node model in the scene
osg::Vec3 nodePosit(0,0,0);
nodeXform->setPosition( nodePosit );
// Declare a 'viewer'
osgViewer::Viewer viewer;
// Next we will need to assign the scene graph we created
// above to this viewer:
viewer.setSceneData( root );
viewer.setCameraManipulator(new osgGA::TrackballManipulator());
viewer.addEventHandler(new osgGA::StateSetManipulator(viewer.getCamera()->getOrCreateStateSet()));
osg::Vec3d position_osg(position[0], position[1], position[2]);
osg::Vec3d target_osg(target[0], target[1], target[2]);
osg::Vec3d up_osg(up[0], up[1], up[2]);
osg::Vec3d view = target_osg - position_osg;
// compute the up and normal vectors
//osg::Quat rot;
// compute the rotation from the view vector to the world up
//rot.makeRotate( up_osg, view ); // #unused
// find the normal vector by crossing the view and world up vectors
osg::Vec3d n = view^up_osg;
// find desired up vector by crossing the normal vector with the view vector
osg::Vec3d up_desired = n^view;
//osg::Vec3d up_new = rot * up_osg; // #unused
// replace the up vector with the desired up
up_osg = up_desired;
// set the camera view
osg::Camera* camera = viewer.getCamera();
camera->setViewMatrixAsLookAt(position_osg, target_osg, up_osg);
// setup the manipulator using the camera, if necessary
viewer.getCameraManipulator()->setHomePosition(position_osg, target_osg, up_osg);
::osg::ref_ptr<SnapImageDrawCallback> snapImageDrawCallback = new SnapImageDrawCallback();
camera->setPostDrawCallback (snapImageDrawCallback.get());
snapImageDrawCallback->setFileName(sFileName_);
snapImageDrawCallback->setSnapImageOnNextFrame(true);
// Add a Light to the scene
osg::ref_ptr<osg::Group> lightGroup (new osg::Group);
osg::ref_ptr<osg::StateSet> lightSS (root->getOrCreateStateSet());
osg::ref_ptr<osg::LightSource> lightSource1 = new osg::LightSource;
double xCenter = 10, yCenter=10;
osg::Vec4f lightPosition (osg::Vec4f(xCenter, yCenter,75,1.0f));
osg::ref_ptr<osg::Light> light = new osg::Light;
light->setLightNum(1);
light->setPosition(lightPosition);
light->setAmbient(osg::Vec4(0.3f,0.3f,0.3f,0.4f));
light->setDiffuse(osg::Vec4(0.2f,0.2f,0.2f,0.5f));
// light->setSpecular(osg::Vec4(0.1,0.1,0.1,0.3));
// light->setConstantAttenuation(0.5f);
light->setDirection(osg::Vec3(0.1f, 0.1f, -1.0f));
lightSource1->setLight(light.get());
lightSource1->setLocalStateSetModes(osg::StateAttribute::ON);
lightSource1->setStateSetModes(*lightSS,osg::StateAttribute::ON);
//osg::StateSet* lightSS (lightGroup->getOrCreateStateSet());
lightGroup->addChild(lightSource1.get());
//Light markers: small spheres
osg::ref_ptr<osg::Geode> lightMarkerGeode (new osg::Geode);
lightMarkerGeode->addDrawable(new osg::ShapeDrawable(new osg::Sphere(osg::Vec3f(xCenter,yCenter,75),10.0f)));
//Tuto 9: lighting code
// root->addChild(lightGroup.get());
//Tuto 9: Adding the light marker geode
// root->addChild(lightMarkerGeode.get());
viewer.realize();
int x,y,width,height;
x = camera->getViewport()->x();
y = camera->getViewport()->y();
width = (WIDTH != 0)? WIDTH : camera->getViewport()->width();
height = (HEIGHT != 0)? HEIGHT : camera->getViewport()->height();
// ::osg::notify(::osg::NOTICE) << "Capturing image from: (" << x << ", " << y<< ") " <<width<< " x "<< height << std::endl;
// Prevent this from opening a window by making pbuffer context
// osg::ref_ptr<osg::GraphicsContext::Traits> traits = new osg::GraphicsContext::Traits;
// traits->x = 0;
// traits->y = 0;
// traits->width = width;
// traits->height = height;
// traits->red = 8;
// traits->green = 8;
// traits->blue = 8;
// traits->alpha = 8;
// traits->windowDecoration = false;
// traits->pbuffer = true;
// traits->doubleBuffer = true;
// traits->sharedContext = 0;
// osg::ref_ptr<osg::GraphicsContext> pbuffer;
// pbuffer = ::osg::GraphicsContext::createGraphicsContext(traits.get());
// if (pbuffer.valid())
// {
// ::osg::notify(osg::NOTICE)<<"Pixel buffer has been created successfully."<<std::endl;
// }
// else
// {
// ::osg::notify(osg::NOTICE)<<"Pixel buffer has not been created successfully."<<std::endl;
// }
// if (pbuffer.valid())
// {
// osg::ref_ptr<osg::Camera> camera = new osg::Camera;
// camera->setGraphicsContext(pbuffer.get());
// camera->setViewport(new osg::Viewport(0,0,width,height));
// GLenum buffer = pbuffer->getTraits()->doubleBuffer ? GL_BACK : GL_FRONT;
// camera->setDrawBuffer(buffer);
// camera->setReadBuffer(buffer);
//// camera->setFinalDrawCallback(new WindowCaptureCallback(mode, position, readBuffer));
// camera->setFinalDrawCallback(snapImageDrawCallback.get());
// viewer.addSlave(camera.get(), osg::Matrixd(), osg::Matrixd());
viewer.realize();
viewer.frame();
// }
}
