void draw()
  {
    ++_frames;
    gpucast::gl::timer_guard full_frame("Frame total");

    float near_clip = 0.01f * _bbox.size().abs();
    float far_clip  = 2.0f  * _bbox.size().abs();

    gpucast::math::matrix4f view = gpucast::math::lookat(0.0f, 0.0f, float(_bbox.size().abs()),
      0.0f, 0.0f, 0.0f,
      0.0f, 1.0f, 0.0f);

    gpucast::math::vec3f translation = _bbox.center();

    gpucast::math::matrix4f model = gpucast::math::make_translation(shiftx(), shifty(), distance()) * rotation() *
      gpucast::math::make_translation(-translation[0], -translation[1], -translation[2]);

    gpucast::math::matrix4f proj = gpucast::math::perspective(50.0f, float(_width) / _height, near_clip, far_clip);

    auto mvp = proj * view * model;
    auto mvpi = gpucast::math::inverse(mvp);

    _renderer->set_nearfar(near_clip, far_clip);
    _renderer->set_resolution(_width, _height);
    _renderer->view_setup(view, model, proj);

    {
      //gpucast::gl::timer_guard t("renderer->begin_draw()");
      _renderer->begin_draw(); 
    }

    for (auto const& o : _objects)
    {
      //gpucast::gl::timer_guard t("Draw object");
      if (_renderer->inside_frustum(*o)) {
        o->draw(*_renderer);
      }
    }

    {
      //gpucast::gl::timer_guard t("renderer->end_draw()");
      _renderer->end_draw();
    }

    {
      gpucast::gl::timer_guard t("fxaa blit");
      glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
      glClearColor(0.4, 0.0, 0.0, 1.0f);
      glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT);

      _fxaa_program->begin();

      _fxaa_program->set_uniform_matrix4fv("modelviewprojectioninverse", 1, false, &mvpi[0]);
      _fxaa_program->set_uniform_matrix4fv("modelviewprojection", 1, false, &mvp[0]);

      _fxaa_program->set_texture2d("colorbuffer", *_colorattachment, 1);
      _fxaa_program->set_texture2d("depthbuffer", *_depthattachment, 2);

      _sample_linear->bind(1);
      _sample_linear->bind(2);

      _fxaa_program->set_uniform1i("fxaa_mode", int(_fxaa));
      _fxaa_program->set_uniform1i("width", GLsizei(_width));
      _fxaa_program->set_uniform1i("height", GLsizei(_height));
      _quad->draw();

      _fxaa_program->end();
    }
  }

void ShapeToGeometryVisitor::apply(const osg::Box& box)
{
    // evaluate hints
    bool createBody = (_hints ? _hints->getCreateBody() : true);
    bool createTop = (_hints ? _hints->getCreateTop() : true);
    bool createBottom = (_hints ? _hints->getCreateBottom() : true);

    float dx = box.getHalfLengths().x();
    float dy = box.getHalfLengths().y();
    float dz = box.getHalfLengths().z();

    gl.PushMatrix();

    gl.Translated(box.getCenter().x(),box.getCenter().y(),box.getCenter().z());

    if (!box.zeroRotation())
    {
        osg::Matrixd rotation(box.computeRotationMatrix());
        gl.MultMatrixd(rotation.ptr());
    }

    gl.Begin(GL_QUADS);

    if (createBody) {
        // -ve y plane
        gl.Normal3f(0.0f,-1.0f,0.0f);

        gl.TexCoord2f(0.0f,1.0f);
        gl.Vertex3f(-dx,-dy,dz);

        gl.TexCoord2f(0.0f,0.0f);
        gl.Vertex3f(-dx,-dy,-dz);

        gl.TexCoord2f(1.0f,0.0f);
        gl.Vertex3f(dx,-dy,-dz);

        gl.TexCoord2f(1.0f,1.0f);
        gl.Vertex3f(dx,-dy,dz);

        // +ve y plane
        gl.Normal3f(0.0f,1.0f,0.0f);

        gl.TexCoord2f(0.0f,1.0f);
        gl.Vertex3f(dx,dy,dz);

        gl.TexCoord2f(0.0f,0.0f);
        gl.Vertex3f(dx,dy,-dz);

        gl.TexCoord2f(1.0f,0.0f);
        gl.Vertex3f(-dx,dy,-dz);

        gl.TexCoord2f(1.0f,1.0f);
        gl.Vertex3f(-dx,dy,dz);

        // +ve x plane
        gl.Normal3f(1.0f,0.0f,0.0f);

        gl.TexCoord2f(0.0f,1.0f);
        gl.Vertex3f(dx,-dy,dz);

        gl.TexCoord2f(0.0f,0.0f);
        gl.Vertex3f(dx,-dy,-dz);

        gl.TexCoord2f(1.0f,0.0f);
        gl.Vertex3f(dx,dy,-dz);

        gl.TexCoord2f(1.0f,1.0f);
        gl.Vertex3f(dx,dy,dz);

        // -ve x plane
        gl.Normal3f(-1.0f,0.0f,0.0f);

        gl.TexCoord2f(0.0f,1.0f);
        gl.Vertex3f(-dx,dy,dz);

        gl.TexCoord2f(0.0f,0.0f);
        gl.Vertex3f(-dx,dy,-dz);

        gl.TexCoord2f(1.0f,0.0f);
        gl.Vertex3f(-dx,-dy,-dz);

        gl.TexCoord2f(1.0f,1.0f);
        gl.Vertex3f(-dx,-dy,dz);
    }

    if (createTop) {
        // +ve z plane
        gl.Normal3f(0.0f,0.0f,1.0f);

        gl.TexCoord2f(0.0f,1.0f);
        gl.Vertex3f(-dx,dy,dz);

        gl.TexCoord2f(0.0f,0.0f);
        gl.Vertex3f(-dx,-dy,dz);

        gl.TexCoord2f(1.0f,0.0f);
        gl.Vertex3f(dx,-dy,dz);

        gl.TexCoord2f(1.0f,1.0f);
        gl.Vertex3f(dx,dy,dz);
//.........这里部分代码省略.........

void TrackBall::stop()
{
    m_rotation = rotation();
    m_paused = true;
}

	RegularData3D* GridAnalysis::calculate()
	{
		// restore the ligand after all calculations, in case the scoring-function is also used for something else...
		AtomContainer* backup_ligand = scoring_function_->getLigand();
		best_poses_.clear();

		scoring_function_->setLigand(probe_group_);

		if (resolution_ < 0) // resolution has not been set manually
		{
			double radius = scoring_function_->getLigandRadius();
			resolution_ = radius/2;
			if (resolution_ < 1) resolution_ = 1;
		}

		const HashGrid3<Atom*>* hashgrid = scoring_function_->getHashGrid();
		origin_ = hashgrid->getOrigin();

		Vector3 hashgrid_resolution = hashgrid->getUnit();
		Size no_x_steps = (Size)((hashgrid->getSizeX()*hashgrid_resolution[0])/resolution_);
		Size no_y_steps = (Size)((hashgrid->getSizeY()*hashgrid_resolution[1])/resolution_);
		Size no_z_steps = (Size)((hashgrid->getSizeZ()*hashgrid_resolution[2])/resolution_);

		Vector3 dimension(no_x_steps*resolution_, no_y_steps*resolution_, no_z_steps*resolution_);
		Vector3 resolution(resolution_, resolution_, resolution_);
		RegularData3D* reg3d = new RegularData3D(origin_, dimension, resolution);

		Size no_atoms = scoring_function_->getNoLigandAtoms();
		bool enable_rotation = 0;
		AtomIterator it = probe_group_.beginAtom();
		if (it == probe_group_.endAtom())
		{
			cerr<<"Error, probe group has no atoms!!"<<endl;
			return 0;
		}
		it++;
		if (it != probe_group_.endAtom()) enable_rotation = 1;

		center_ = scoring_function_->getLigandCenter();

		for (Size x = 0; x < no_x_steps; x++)
		{
			for (Size y = 0; y < no_y_steps; y++)
			{

				for (Size z = 0; z < no_z_steps; z++)
				{
					Vector3 position(origin_[0]+(x+0.5)*resolution_, origin_[1]+(y+0.5)*resolution_, origin_[2]+(z+0.5)*resolution_);

					moveProbeGroup_(position);

					scoring_function_->update();
					double score = scoring_function_->updateScore();
					int best_angle_x = 0;
					int best_angle_y = 0;
					int best_angle_z = 0;

					// if probe-group has more than one atom, enable rotation around all three axes and store minimal score in RegularData3D
					if (enable_rotation && score < 0.75e10*no_atoms)
					{
						for (Size i = 1; i < 36; i++)
						{
							rotateProbeGroup_(0, 10);
							scoring_function_->update();
							double score_i = scoring_function_->updateScore();
							if (score_i < score)
							{
								score = score_i;
								best_angle_x = i*10;
							}
						}
						rotateProbeGroup_(0, -350+best_angle_x);
						for (Size i = 1; i < 36; i++)
						{
							rotateProbeGroup_(1, 10);
							scoring_function_->update();
							double score_i = scoring_function_->updateScore();
							if (score_i < score)
							{
								score = score_i;
								best_angle_y = i*10;
							}
						}
						rotateProbeGroup_(1, -350+best_angle_y);
						for (Size i = 1; i < 36; i++)
						{
							rotateProbeGroup_(2, 10);
							scoring_function_->update();
							double score_i = scoring_function_->updateScore();
							if (score_i < score)
							{
								score = score_i;
								best_angle_z = i*10;
							}
						}

						// rotate back to original orientation
						rotateProbeGroup_(2, -350);
						rotateProbeGroup_(1, -best_angle_y);
						rotateProbeGroup_(0, -best_angle_x);
//.........这里部分代码省略.........

RobotMeshModel::RobotMeshModel():nh_priv_("~") {

    //Load robot description from parameter server
    std::string robot_desc_string;
    if(!nh_.getParam("robot_description", robot_desc_string)) {
        ROS_ERROR("Could not get urdf from param server");

    }


    if (!urdf_.initString(robot_desc_string)) {
        ROS_ERROR("Failed to parse urdf");

    }
    modelframe_= "/torso_lift_link";

    nh_priv_.param<std::string>("robot_description_package_path", description_path, "..");
    ROS_INFO("package_path %s", description_path.c_str());
    nh_priv_.param<std::string>("camera_topic", camera_topic_, "/wide_stereo/right" );
    nh_priv_.param<std::string>("camera_info_topic", camera_info_topic_, "/wide_stereo/right/camera_info" );



    //Load robot mesh for each link
    std::vector<boost::shared_ptr<urdf::Link> > links ;
    urdf_.getLinks(links);
    for (int i=0; i< links.size(); i++) {
        if (links[i]->visual.get() == NULL) continue;
        if (links[i]->visual->geometry.get() == NULL) continue;
        if (links[i]->visual->geometry->type == urdf::Geometry::MESH) {

            //todo: this should really be done by resource retriever
            boost::shared_ptr<urdf::Mesh> mesh = boost::dynamic_pointer_cast<urdf::Mesh> (links[i]->visual->geometry);
            std::string filename (mesh->filename);

            if (filename.substr(filename.size() - 4 , 4) != ".stl" && filename.substr(filename.size() - 4 , 4) != ".dae") continue;
            if (filename.substr(filename.size() - 4 , 4) == ".dae")
                filename.replace(filename.size() - 4 , 4, ".stl");
            ROS_INFO("adding link %d %s",i,links[i]->name.c_str());
            filename.erase(0,25);
            filename = description_path + filename;

            boost::shared_ptr<CMeshO> mesh_ptr(new CMeshO);

            if(vcg::tri::io::ImporterSTL<CMeshO>::Open(*mesh_ptr,filename.c_str())) {
                ROS_ERROR("could not load mesh %s", filename.c_str());
                continue;
            }

            links_with_meshes.push_back(links[i]);
            meshes[links[i]->name] = mesh_ptr;

            tf::Vector3 origin(links[i]->visual->origin.position.x, links[i]->visual->origin.position.y, links[i]->visual->origin.position.z);
            tf::Quaternion rotation(links[i]->visual->origin.rotation.x, links[i]->visual->origin.rotation.y, links[i]->visual->origin.rotation.z, links[i]->visual->origin.rotation.w);

            offsets_[links[i]->name] = tf::Transform(rotation, origin);





        }

    }


    initRobot();

    //get camera intinsics

    ROS_INFO("waiting for %s", camera_info_topic_.c_str());
    cam_info_ = ros::topic::waitForMessage<sensor_msgs::CameraInfo>(camera_info_topic_);
    cameraframe_ = cam_info_->header.frame_id;

    ROS_INFO("%s: robot model initialization done!", ros::this_node::getName().c_str());

}

//.........这里部分代码省略.........
                    break;
                }
		    }
		    if (finiteIndex == -1)
		    {
                // there are no finite points, which means camera doesn't see plane of interest.
                // so we don't care what the shadow map matrix is
                // We'll map points off the shadow map so they aren't even stored
                Matrix4 crazyMat(0.0, 0.0, 0.0, 5.0,
                                 0.0, 0.0, 0.0, 5.0,
                                 0.0, 0.0, 0.0, 5.0,
                                 0.0, 0.0, 0.0, 1.0);
                texCam->setCustomViewMatrix(true, Matrix4::IDENTITY);
                texCam->setCustomProjectionMatrix(true, crazyMat);	
                return;
		    }
            // swap finite point to last point
            std::swap(vhull[3], vhull[finiteIndex]);
        }
        vhull.resize(4);

		// get the post-projective coordinate constraints
		vector<Vector2>::type constraint;
		for (int i=0; i<4; i++)
		{
			Vector4 postProjPt = camProjection * vhull[i];
			postProjPt *= 1.0 / postProjPt.w;
			constraint.push_back(Vector2(postProjPt.x, postProjPt.y));
		}

		// perturb one point so we don't have coplanarity
		const Vector4& pinhole = light->getAs4DVector();
		const Vector4& oldPt = vhull.back();
        Vector4 newPt;
        if (pinhole.w == 0)
        {
            // It's directional light
            static const Real NEAR_SCALE = 100.0;
            newPt = oldPt + (pinhole * (cam->getNearClipDistance() * NEAR_SCALE));
        }
        else
        {
            // It's point or spotlight
		    Vector4 displacement = oldPt - pinhole;
		    Vector3 displace3    = Vector3(displacement.x, displacement.y, displacement.z);
		    Real dotProd = fabs(displace3.dotProduct(worldPlane.normal));
		    static const Real NEAR_FACTOR = 0.05;
		    newPt = pinhole + (displacement * (cam->getNearClipDistance() * NEAR_FACTOR / dotProd));
        }
		vhull.back() = newPt;

		// solve for the matrix that stabilizes the plane
		Matrix4 customMatrix = computeConstrainedProjection(pinhole, vhull, constraint);

        if (pinhole.w == 0)
        {
            // TODO: factor into view and projection pieces.
            // Note: In fact, it's unnecessary to factor into view and projection pieces,
            // but if we do, we will more according with academic requirement :)
            texCam->setCustomViewMatrix(true, Matrix4::IDENTITY);
            texCam->setCustomProjectionMatrix(true, customMatrix);
            return;
        }

        Vector3 tempPos = Vector3(pinhole.x, pinhole.y, pinhole.z);

		// factor into view and projection pieces
		Matrix4    translation(1.0, 0.0, 0.0,  tempPos.x,
			0.0, 1.0, 0.0,  tempPos.y,
			0.0, 0.0, 1.0,  tempPos.z,
			0.0, 0.0, 0.0,  1.0);
		Matrix4 invTranslation(1.0, 0.0, 0.0, -tempPos.x,
			0.0, 1.0, 0.0, -tempPos.y,
			0.0, 0.0, 1.0, -tempPos.z,
			0.0, 0.0, 0.0,  1.0);
		Matrix4 tempMatrix = customMatrix * translation;
		Vector3 zRow(-tempMatrix[3][0], -tempMatrix[3][1], -tempMatrix[3][2]);
		zRow.normalise();
		Vector3 up;
		if (zRow.y == 1.0)
			up = Vector3(1,0,0);
		else
			up = Vector3(0,1,0);
		Vector3 xDir = up.crossProduct(zRow);
		xDir.normalise();
		up = zRow.crossProduct(xDir);
		Matrix4 rotation(xDir.x, up.x, zRow.x, 0.0,
			xDir.y, up.y, zRow.y, 0.0,
			xDir.z, up.z, zRow.z, 0.0,
			0.0,  0.0,    0.0, 1.0 );
		Matrix4 customProj = tempMatrix * rotation;
		Matrix4 customView = rotation.transpose() * invTranslation;
		// note: now customProj * (0,0,0,1)^t = (0, 0, k, 0)^t for k some constant
		// note: also customProj's 4th row is (0, 0, c, 0) for some negative c.


		// set the shadow map camera
		texCam->setCustomViewMatrix(true, customView);
		texCam->setCustomProjectionMatrix(true, customProj);
	}

void QoccHarnessWindow::createActions()
{
    newAction = new QAction(tr("&New"), this);
    newAction->setShortcut(tr("Ctrl+N"));
    newAction->setStatusTip(tr("Create a new file"));
    connect(newAction, SIGNAL(triggered()), this, SLOT(newFile()));

    openAction = new QAction(tr("&Open..."), this);
    openAction->setShortcut(tr("Ctrl+O"));
    openAction->setStatusTip(tr("Open an existing file"));
    connect(openAction, SIGNAL(triggered()), this, SLOT(open()));

    saveAction = new QAction(tr("&Save"), this);
    saveAction->setShortcut(tr("Ctrl+S"));
    saveAction->setStatusTip(tr("Save the document to disk"));
    connect(saveAction, SIGNAL(triggered()), this, SLOT(save()));

    printAction = new QAction(tr("&Print..."), this);
    printAction->setShortcut(tr("Ctrl+P"));
    printAction->setStatusTip(tr("Print the document"));
    connect(printAction, SIGNAL(triggered()), this, SLOT(print()));

    exitAction = new QAction(tr("E&xit"), this);
    exitAction->setShortcut(tr("Ctrl+X"));
    exitAction->setStatusTip(tr("Exit the application"));
    connect(exitAction, SIGNAL(triggered()), this, SLOT(close()));

    undoAction = new QAction(tr("&Undo"), this);
    undoAction->setShortcut(tr("Ctrl+Z"));
    undoAction->setStatusTip(tr("Undo the last operation"));
    connect(undoAction, SIGNAL(triggered()), this, SLOT(undo()));

    redoAction = new QAction(tr("&Redo"), this);
    redoAction->setShortcut(tr("Ctrl+Y"));
    redoAction->setStatusTip(tr("Redo the last operation"));
    connect(redoAction, SIGNAL(triggered()), this, SLOT(redo()));

    cutAction = new QAction(tr("Cu&t"), this);
    cutAction->setShortcut(tr("Ctrl+X"));
    cutAction->setStatusTip(tr("Cut the current selection's contents to the clipboard"));
    connect(cutAction, SIGNAL(triggered()), this, SLOT(cut()));

    copyAction = new QAction(tr("&Copy"), this);
    copyAction->setShortcut(tr("Ctrl+C"));
    copyAction->setStatusTip(tr("Copy the current selection's contents to the clipboard"));
    connect(copyAction, SIGNAL(triggered()), this, SLOT(copy()));

    pasteAction = new QAction(tr("&Paste"), this);
    pasteAction->setShortcut(tr("Ctrl+V"));
    pasteAction->setStatusTip(tr("Paste the clipboard's contents into the current selection"));
    connect(pasteAction, SIGNAL(triggered()), this, SLOT(paste()));

    aboutAction = new QAction(tr("&About"), this);
    aboutAction->setStatusTip(tr("Show the application's About box"));
    connect(aboutAction, SIGNAL(triggered()), this, SLOT(about()));

    aboutQtAction = new QAction(tr("About &Qt"), this);
    aboutQtAction->setStatusTip(tr("Show the Qt library's About box"));
    connect(aboutQtAction, SIGNAL(triggered()), qApp, SLOT(aboutQt()));
    connect(aboutQtAction, SIGNAL(triggered()), this, SLOT(aboutQt()));

	// Now for the QtOCCViewWidget slots.
/*
	fitAction = new QAction(tr("&Fit Window"), this);
	fitAction->setShortcut(tr("Ctrl+F"));
    fitAction->setStatusTip(tr("Fit to window"));
    connect(fitAction, SIGNAL(triggered()), myOCC, SLOT(fitExtents()));
*/
	fitAllAction = new QAction(tr("&Fit All"), this);
	fitAllAction->setShortcut(tr("Ctrl+F"));
    fitAllAction->setStatusTip(tr("Fit contents to viewport"));
    connect(fitAllAction, SIGNAL(triggered()), myOCC, SLOT(fitAll()));

	zoomAction = new QAction(tr("&Zoom"), this);
	zoomAction->setStatusTip(tr("Zoom in window"));
    connect(zoomAction, SIGNAL(triggered()), myOCC, SLOT(fitArea()));

	panAction = new QAction(tr("&Pan"), this);
    panAction->setStatusTip(tr("Window panning"));
    connect(panAction, SIGNAL(triggered()), myOCC, SLOT(pan()));

	rotAction = new QAction(tr("&Rotate"), this);
	rotAction->setShortcut(tr("Ctrl+R"));
    rotAction->setStatusTip(tr("Window rotation"));
    connect(rotAction, SIGNAL(triggered()), myOCC, SLOT(rotation()));

	gridToggleAction = new QAction(tr("Toggle &Grid"), this);
	gridToggleAction->setShortcut(tr("Ctrl+G"));
    gridToggleAction->setStatusTip(tr("Turn the grid on or off"));
    connect(gridToggleAction, SIGNAL(triggered()), myVC, SLOT(gridToggle()));

/*	gridOffAction = new QAction(tr("Gri&d Off"), this);
	gridOffAction->setShortcut(tr("Ctrl+D"));
    gridOffAction->setStatusTip(tr("Turn the grid on"));
    connect(gridOffAction, SIGNAL(triggered()), myVC, SLOT(gridOff()));
*/
	gridXYAction = new QAction(tr("XY Grid"), this);
    gridXYAction->setStatusTip(tr("Grid on XY Plane"));
	//gridOffAction->setShortcut(tr("Ctrl+Z"));
    connect(gridXYAction, SIGNAL(triggered()), myVC, SLOT(gridXY()));
//.........这里部分代码省略.........

//.........这里部分代码省略.........
        const mwSize *indicesDim = mxGetDimensions(prhs[1]);
        const mwSize *priorDim = mxGetDimensions(prhs[4]);

        // Now check them
        if( ndimensions1 != 2 || ndimensions2 != 2 || ndimensions3 != 2 || ndimensions4 != 2 ||
                (data1dim[0] != 3 && data1dim[0] != 6) ||
                (data2dim[0] != 3 && data2dim[0] != 6) ||
                indicesDim[0] != 1 ||
                priorDim[0] != 3 ||
                (priorDim[1] != 1 &&  priorDim[1] != 3 && priorDim[1] != 4) ||
                data1dim[1] != data2dim[1] ||
                data1dim[1] < 1 || data2dim[1] < 1 ||
                data2dim[1] < indicesDim[1] )
        {
            mexPrintf("opengv: Bad input to mex function opengv\n");
            mexPrintf("Assuming signature: X = opengv( method, indices, data1, ");
            mexPrintf("data2, prior )\n");
            mexPrintf("Inputs data1 and data2 must have size (3,n) or (6,n),\n");
            mexPrintf("with an equal number of columns\n");
            mexPrintf("indices must be a 1xm vector, with m smaller or equal than n\n");
            mexPrintf("prior must be a 3x1, 3x3, or 3x4 matrix\n");
            return;
        }

        callCharacter = 2;
    }

    //create three pointers to absolute, relative, and point_cloud adapters here
    opengv::absolute_pose::AbsoluteAdapterBase* absoluteAdapter;
    opengv::relative_pose::RelativeAdapterBase* relativeAdapter;
    opengv::point_cloud::PointCloudAdapterBase* pointCloudAdapter;

    int translationPrior = 0;
    int rotationPrior = 0;
    opengv::translation_t translation;
    opengv::rotation_t rotation;

    //set the prior if needed
    if( callCharacter == 2 )
    {
        const mxArray *prior;
        const mwSize *priorDim;

        prior = prhs[4];
        priorDim = mxGetDimensions(prhs[4]);

        if( priorDim[1] == 1 )
        {
            //set translation
            translationPrior = 1;
            double * ptr = (double*) mxGetData(prior);
            translation[0] = ptr[0];
            translation[1] = ptr[1];
            translation[2] = ptr[2];
        }
        if( priorDim[1] == 3 )
        {
            //set rotation
            rotationPrior = 1;
            double * ptr = (double*) mxGetData(prior);
            rotation(0,0) = ptr[0];
            rotation(1,0) = ptr[1];
            rotation(2,0) = ptr[2];
            rotation(0,1) = ptr[3];
            rotation(1,1) = ptr[4];
            rotation(2,1) = ptr[5];

//.........这里部分代码省略.........
                palm->setSixenseID(data->controller_index);
                qCDebug(interfaceapp, "Found new Sixense controller, ID %i", data->controller_index);
            }
            
            // Disable the hands (and return to default pose) if both controllers are at base station
            if (foundHand) {
                palm->setActive(!_controllersAtBase);
            } else {
                palm->setActive(false); // if this isn't a Sixsense ID palm, always make it inactive
            }
            
            
            //  Read controller buttons and joystick into the hand
            palm->setControllerButtons(data->buttons);
            palm->setTrigger(data->trigger);
            palm->setJoystick(data->joystick_x, data->joystick_y);
            
            // Emulate the mouse so we can use scripts
            if (Menu::getInstance()->isOptionChecked(MenuOption::SixenseMouseInput) && !_controllersAtBase) {
                emulateMouse(palm, numActiveControllers - 1);
            }
            
            // NOTE: Sixense API returns pos data in millimeters but we IMMEDIATELY convert to meters.
            glm::vec3 position(data->pos[0], data->pos[1], data->pos[2]);
            position *= METERS_PER_MILLIMETER;
            
            // Check to see if this hand/controller is on the base
            const float CONTROLLER_AT_BASE_DISTANCE = 0.075f;
            if (glm::length(position) < CONTROLLER_AT_BASE_DISTANCE) {
                numControllersAtBase++;
            }
            
            // Transform the measured position into body frame.
            glm::vec3 neck = _neckBase;
            // Zeroing y component of the "neck" effectively raises the measured position a little bit.
            neck.y = 0.0f;
            position = _orbRotation * (position - neck);
            
            //  Rotation of Palm
            glm::quat rotation(data->rot_quat[3], -data->rot_quat[0], data->rot_quat[1], -data->rot_quat[2]);
            rotation = glm::angleAxis(PI, glm::vec3(0.0f, 1.0f, 0.0f)) * _orbRotation * rotation;
            
            //  Compute current velocity from position change
            glm::vec3 rawVelocity;
            if (deltaTime > 0.0f) {
                rawVelocity = (position - palm->getRawPosition()) / deltaTime;
            } else {
                rawVelocity = glm::vec3(0.0f);
            }
            palm->setRawVelocity(rawVelocity);   //  meters/sec
            
            // adjustment for hydra controllers fit into hands
            float sign = (i == 0) ? -1.0f : 1.0f;
            rotation *= glm::angleAxis(sign * PI/4.0f, glm::vec3(0.0f, 0.0f, 1.0f));
            
            //  Angular Velocity of Palm
            glm::quat deltaRotation = rotation * glm::inverse(palm->getRawRotation());
            glm::vec3 angularVelocity(0.0f);
            float rotationAngle = glm::angle(deltaRotation);
            if ((rotationAngle > EPSILON) && (deltaTime > 0.0f)) {
                angularVelocity = glm::normalize(glm::axis(deltaRotation));
                angularVelocity *= (rotationAngle / deltaTime);
                palm->setRawAngularVelocity(angularVelocity);
            } else {
                palm->setRawAngularVelocity(glm::vec3(0.0f));
            }
            
            if (_lowVelocityFilter) {
                //  Use a velocity sensitive filter to damp small motions and preserve large ones with
                //  no latency.
                float velocityFilter = glm::clamp(1.0f - glm::length(rawVelocity), 0.0f, 1.0f);
                position = palm->getRawPosition() * velocityFilter + position * (1.0f - velocityFilter);
                rotation = safeMix(palm->getRawRotation(), rotation, 1.0f - velocityFilter);
                palm->setRawPosition(position);
                palm->setRawRotation(rotation);
            } else {
                palm->setRawPosition(position);
                palm->setRawRotation(rotation);
            }
            
            // Store the one fingertip in the palm structure so we can track velocity
            const float FINGER_LENGTH = 0.3f;   //  meters
            const glm::vec3 FINGER_VECTOR(0.0f, 0.0f, FINGER_LENGTH);
            const glm::vec3 newTipPosition = position + rotation * FINGER_VECTOR;
            glm::vec3 oldTipPosition = palm->getTipRawPosition();
            if (deltaTime > 0.0f) {
                palm->setTipVelocity((newTipPosition - oldTipPosition) / deltaTime);
            } else {
                palm->setTipVelocity(glm::vec3(0.0f));
            }
            palm->setTipPosition(newTipPosition);
        }
        
        if (numActiveControllers == 2) {
            updateCalibration(controllers);
        }
        _controllersAtBase = (numControllersAtBase == 2);
    }
#endif  // HAVE_SIXENSE
}

int Node::getNodeRotationAngle () const
{
    return static_cast<int>(rotation());
}

//.........这里部分代码省略.........
					    target_pose.header.stamp, ros::Duration(1),
					    ros::Duration(0.01), &err);
	if (!waiting_result) {
	    std::cerr<<"Wait for transform err: "<<err<<"\n";
	    continue;
	}
	
	geometry_msgs::PoseStamped result;	
	transformer.transformPose("/schunk/position/GripperBox",target_pose,result);
	
	double gripper_desired_roll = -atan2(result.pose.position.y, result.pose.position.z);
	double gripper_desired_pitch = atan2(result.pose.position.x, result.pose.position.z);
// 	double gripper_desired_pitch = 0;
	double gripper_desired_yaw = 0;	
	
	
	ROS_DEBUG("From the gripper point of view (roll,pitch,yaw): %.2f, %.2f, %.2f", 
						     gripper_desired_roll, 
						     gripper_desired_pitch,
						     gripper_desired_yaw);
	
//	geometry_msgs::QuaternionStamped rot_request, rot_result;
//	rot_request.header.frame_id = "/schunk/position/GripperBox";
//	rot_request.header.stamp = target_pose.header.stamp;
	
//	tf::Quaternion rot_quaternion;
//	rot_quaternion.setRPY(gripper_desired_roll, gripper_desired_pitch, gripper_desired_yaw);
	
//	rot_request.quaternion.x=rot_quaternion.getX();
//	rot_request.quaternion.y=rot_quaternion.getY();
//	rot_request.quaternion.z=rot_quaternion.getZ();
//	rot_request.quaternion.w=rot_quaternion.getW();
	
	tf::Vector3 rotation(gripper_desired_roll, gripper_desired_pitch, gripper_desired_yaw);
	tf::StampedTransform transform;
	transformer.waitForTransform("/schunk/position/PAM112_BaseConector", "/schunk/position/GripperBox", 
					    target_pose.header.stamp, ros::Duration(5.0));
	transformer.lookupTransform("/schunk/position/PAM112_BaseConector", "/schunk/position/GripperBox", target_pose.header.stamp , transform);
	tf::Vector3 result_vector = transform.getBasis() * rotation;
	
	
//	transformer.waitForTransform("/schunk/position/GripperBox", "/schunk/position/PAM112_BaseConector", 
//					    rot_request.header.stamp, ros::Duration(1.0));
//	transformer.transformQuaternion("/schunk/position/PAM112_BaseConector",rot_request,rot_result);

	//getting the velocity vector
	
//	ROS_INFO("Target pose (x,y,z): %.2f, %.2f, %.2f", result.pose.position.x, 
//						     result.pose.position.y, result.pose.position.z);
						     
	ROS_DEBUG("Target anglulars (x,y,z): %.2f, %.2f, %.2f", result_vector.x(), 
						     result_vector.y(),result_vector.z());

//	tf::Quaternion q(rot_result.quaternion.x, rot_result.quaternion.y,
//		       rot_result.quaternion.z, rot_result.quaternion.w);
	
//	double gripper_roll, gripper_pitch, gripper_yaw	       ;
//	btMatrix3x3(q).getRPY(gripper_roll, gripper_pitch, gripper_yaw);
	
//	ROS_INFO("Gripper orientation: (roll,pitch,yaw): %.2f, %.2f, %.2f", 
//						     gripper_roll, 
//						     gripper_pitch,
//						     gripper_yaw);	


	double gripper_roll, gripper_pitch, gripper_yaw;

/*!
sets the rotation of this node. The axis parameter
will be transformed into 'relativeTo' space. So a passing in an axis
of (0, 1, 0) with TS_Object will rotate the node around its own up axis.
*/
void SLNode::rotation(SLfloat angleDeg, const SLVec3f& axis, 
                      SLTransformSpace relativeTo)
{    
    SLQuat4f rot(angleDeg, axis);
    rotation(rot, relativeTo);
}

int main(int, char const**)
{
    // Create the main window
    sf::RenderWindow window(sf::VideoMode(800, 600), "SFML window");

    /**
     * Bruit de base
     */
    
    sf::Image imagebruitRandom;
    sf::Texture textureBruitRandom;
    sf::Sprite spriteBruitRandom;
    imagebruitRandom.create(800, 600);
    
    for(int i = 0; i < 800; i++)
        for(int j = 0; j < 600; j++)
            if(getIntRandom(0,1) == 1)
                imagebruitRandom.setPixel(i,j, sf::Color::White);
    
    textureBruitRandom.loadFromImage(imagebruitRandom);
    spriteBruitRandom.setTexture(textureBruitRandom);

    /*-----------------------*/
 
    /**
     * Le background
     */
    
    sf::Image backgroundMouvement = imagebruitRandom;
    sf::Texture textureBackgroundMouvement;
    sf::Sprite spriteBackgroundMouvement;
    
    /*------------------------*/
    
    /**
     * Le texte
     */
    
    sf::Text texteEcrit;
    sf::Font fontTexte;
    fontTexte.loadFromFile("/Users/nicolasserf/Desktop/ProjetPersonnel/monPremierBruit/monPremierBruit/sansation.ttf");
    
    texteEcrit.setCharacterSize(100);
    texteEcrit.setColor(sf::Color::White);
    texteEcrit.setString("SALOPE");
    texteEcrit.setFont(fontTexte);
    texteEcrit.setPosition(250, 300);
    
    /*-------------------------*/
    
    /**
     * Texture texte "bruité"
     */
    
    sf::RenderTexture texteBruite;
    texteBruite.create(800, 600);
    texteBruite.draw(texteEcrit);
    texteBruite.display();
    texteBruite.draw(spriteBruitRandom, sf::BlendMultiply);
    texteBruite.display();
    
    sf::Sprite spriteTexteBruite;
    spriteTexteBruite.setTexture(texteBruite.getTexture());
    
    /*-------------------------*/
    
    /**
     * Texture background "bruité"
     */
    
    texteEcrit.setColor(sf::Color::Black); //on va écrire en noir sur fond blanc
    sf::RenderTexture backgroundBruite;
    backgroundBruite.create(800, 600);
    backgroundBruite.clear(sf::Color::White); //Fond blanc
    backgroundBruite.draw(texteEcrit);
    backgroundBruite.display();
    
    /*------------------------*/
    
    /**
     * Réunion du texte bruité et du background bruité
     */
    
    sf::Sprite regroupe;
    
    bool stop = true;

    while (window.isOpen())
    {
        if(stop)
        {
        backgroundMouvement = rotation(backgroundMouvement);
        textureBackgroundMouvement.loadFromImage(backgroundMouvement);
        spriteBackgroundMouvement.setTexture(textureBackgroundMouvement);
        
        backgroundBruite.draw(spriteBackgroundMouvement, sf::BlendMultiply);
        backgroundBruite.display();
        regroupe.setTexture(backgroundBruite.getTexture());
        
        backgroundBruite.draw(spriteTexteBruite, sf::BlendAdd);
//.........这里部分代码省略.........

bool RotateDesktopGesture::processGestureImpl(GestureContext *gestureContext)
{
	float angle = calculateAngle(gestureContext);
	QList<Path *> activePaths = gestureContext->getActiveTouchPaths();
	if (_animatingToForward)
	{
		// user may realize accidental trigger of rotation while camera is orienting to face "forward" wall
		if (gestureContext->getNumActiveTouchPoints() != 2) 
		{
			_animatingToForward = false;
			cam->killAnimation();
			cam->animateTo(_startPos, _startDir, _startUp);
			return false;
		}

		if(cam->isAnimating())
			return true; // let camera animate to face the "forward" wall first
		else
		{
			_originalDir = cam->getDir();
			_originalUp = cam->getUp();
			_compensate = angle;
			_animatingToForward = false;
		}
	}

	if (gestureContext->getNumActiveTouchPoints() != 2) 
	{
		if (activePaths.count() == 1)
		{
			if (activePaths.contains(_gestureTouchPaths[0]) || activePaths.contains(_gestureTouchPaths[1]))
			{
				_recalculateInitials = true;
				return true; // still remain as rotate gesture, user probably lifted and repositioning one finger
			}
		}
		if (abs(angle) < SNAP_MINIMUM_ANGLE) // finished rotation gesture, check if a rotation is achieved
		{
			cam->animateTo(_startPos, _startDir, _startUp); // rotation amount too small, revert to original view
			return false;
		}
		// Snap the camera to the nearest wall
		int wallIndex = getWallCameraIsFacing();
		assert(wallIndex != -1);
		Vec3 camDir, camPos;
		cam->lookAtWall(GLOBAL(Walls)[wallIndex], camPos, camDir);
		cam->animateTo(camPos, camDir);
		return false;
	}

	if (_recalculateInitials)
	{
		_recalculateInitials = false;
		// if rotation after repositioning is canceled, revert camera to before lifting and repositioning
		// not revert camera to before rotation gesture is recognized
		_startUp = cam->getUp(); 
		_startDir = cam->getDir();
		_startPos = cam->getEye();
	}

	cam->revertAnimation();
	
	pair<Vec3, Vec3> camPair = calculateCameraPosition();
	cam->setEye(camPair.first);
	
	// Rotate the camera; take out the amount used to trigger rotation and face "forward" wall to avoid too much initial rotation
	float rotationAngle = angle - _compensate;
	Quat rotation(rotationAngle, Vec3(0.0f, 1.0f, 0.0f));
	Vec3 dir = _originalDir;
	Vec3 up = _originalUp;
	rotation.rotate(dir);
	rotation.rotate(up);

	dir.sety(camPair.second.y);
	
	cam->animateTo(cam->getEye(), dir, up, 5, false);	
	
	return true;
}

void CameraManager::SetupPerspectiveModelView() {

	// select the view matrix
	glMatrixMode(GL_MODELVIEW);
	// set it to '1'
	glLoadIdentity();

	// our values represent the angles in degrees, but 3D 
	// math typically demands angular values are in radians.
	float pitch = m_cameraPitch * RADIANS_PER_DEGREE;
	float yaw = m_cameraYaw * RADIANS_PER_DEGREE;

	// create a quaternion defining the angular rotation 
	// around the up vector
	btQuaternion rotation(m_upVector, yaw);

	// set the camera's position to 0,0,0, then move the 'z' 
	// position to the current value of m_cameraDistance.
	btVector3 cameraPosition(0, 0, 0);
	//cameraPosition[2] = -m_cameraDistance;
	cameraPosition[2] = m_cameraDistance;

	// Translation
	m_cameraTarget[0] = m_cameraPosX;
	m_cameraTarget[1] = m_cameraPosY;

	// create a Bullet Vector3 to represent the camera 
	// position and scale it up if its value is too small.
	btVector3 forward(cameraPosition[0], cameraPosition[1], cameraPosition[2]);
	if (forward.length2() < SIMD_EPSILON) {
		forward.setValue(1.f, 0.f, 0.f);
	}

	// figure out the 'right' vector by using the cross 
	// product on the 'forward' and 'up' vectors
	btVector3 right = m_upVector.cross(forward);

	// create a quaternion that represents the camera's roll
	btQuaternion roll(right, -pitch);

	// turn the rotation (around the Y-axis) and roll (around 
	// the forward axis) into transformation matrices and 
	// apply them to the camera position. This gives us the 
	// final position
	cameraPosition = btMatrix3x3(rotation) * btMatrix3x3(roll) * cameraPosition;

	// save our new position in the member variable, and 
	// shift it relative to the target position (so that we 
	// orbit it)
	m_cameraPosition[0] = cameraPosition.getX();
	m_cameraPosition[1] = cameraPosition.getY();
	m_cameraPosition[2] = cameraPosition.getZ();
	m_cameraPosition += m_cameraTarget;

	// create a view matrix based on the camera's position and where it's
	// looking
	//printf("Camera Position = %f, %f, %f\n", cameraPosition[0], cameraPosition[1], cameraPosition[2]);
	// the view matrix is now set
	gluLookAt(m_cameraPosition[0], m_cameraPosition[1], m_cameraPosition[2], m_cameraTarget[0], m_cameraTarget[1], m_cameraTarget[2], m_upVector.getX(), m_upVector.getY(), m_upVector.getZ());

}

void SixenseManager::update(float deltaTime) {
#ifdef HAVE_SIXENSE
    // if the controllers haven't been moved in a while, disable
    const unsigned int MOVEMENT_DISABLE_SECONDS = 3;
    if (usecTimestampNow() - _lastMovement > (MOVEMENT_DISABLE_SECONDS * USECS_PER_SECOND)) {
        Hand* hand = Application::getInstance()->getAvatar()->getHand();
        for (std::vector<PalmData>::iterator it = hand->getPalms().begin(); it != hand->getPalms().end(); it++) {
            it->setActive(false);
        }
        _lastMovement = usecTimestampNow();
    }

    if (sixenseGetNumActiveControllers() == 0) {
        _hydrasConnected = false;
        return;
    } 

    PerformanceTimer perfTimer("sixense");
    if (!_hydrasConnected) {
        _hydrasConnected = true;
        UserActivityLogger::getInstance().connectedDevice("spatial_controller", "hydra");
    }
    MyAvatar* avatar = Application::getInstance()->getAvatar();
    Hand* hand = avatar->getHand();
    
    int maxControllers = sixenseGetMaxControllers();

    // we only support two controllers
    sixenseControllerData controllers[2];

    int numActiveControllers = 0;
    for (int i = 0; i < maxControllers && numActiveControllers < 2; i++) {
        if (!sixenseIsControllerEnabled(i)) {
            continue;
        }
        sixenseControllerData* data = controllers + numActiveControllers;
        ++numActiveControllers;
        sixenseGetNewestData(i, data);
        
        //  Set palm position and normal based on Hydra position/orientation
        
        // Either find a palm matching the sixense controller, or make a new one
        PalmData* palm;
        bool foundHand = false;
        for (size_t j = 0; j < hand->getNumPalms(); j++) {
            if (hand->getPalms()[j].getSixenseID() == data->controller_index) {
                palm = &(hand->getPalms()[j]);
                foundHand = true;
            }
        }
        if (!foundHand) {
            PalmData newPalm(hand);
            hand->getPalms().push_back(newPalm);
            palm = &(hand->getPalms()[hand->getNumPalms() - 1]);
            palm->setSixenseID(data->controller_index);
            qDebug("Found new Sixense controller, ID %i", data->controller_index);
        }
        
        palm->setActive(true);
        
        //  Read controller buttons and joystick into the hand
        palm->setControllerButtons(data->buttons);