Beispiel #1
0
  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();
    }
  }
Beispiel #2
0
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);
    }

    if (createBottom) {
        // -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);
    }

    gl.End();

    gl.PopMatrix();

}
Beispiel #3
0
void ShapeToGeometryVisitor::apply(const osg::Cylinder& cylinder)
{
    gl.PushMatrix();

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

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

    // evaluate hints
    bool createBody = (_hints ? _hints->getCreateBody() : true);
    bool createTop = (_hints ? _hints->getCreateTop() : true);
    bool createBottom = (_hints ? _hints->getCreateBottom() : true);

    unsigned int numSegments = 40;
    float ratio = (_hints ? _hints->getDetailRatio() : 1.0f);
    if (ratio > 0.0f && ratio != 1.0f) {
        numSegments = (unsigned int) (numSegments * ratio);
        if (numSegments < MIN_NUM_SEGMENTS)
            numSegments = MIN_NUM_SEGMENTS;
    }


    // cylinder body
    if (createBody)
        drawCylinderBody(numSegments, cylinder.getRadius(), cylinder.getHeight());

    float angleDelta = 2.0f*osg::PI/(float)numSegments;
    float texCoordDelta = 1.0f/(float)numSegments;

    float r = cylinder.getRadius();
    float h = cylinder.getHeight();

    float basez = -h*0.5f;
    float topz = h*0.5f;

    float angle = 0.0f;
    float texCoord = 0.0f;

    // cylinder top
    if (createTop) {
        gl.Begin(GL_TRIANGLE_FAN);

        gl.Normal3f(0.0f,0.0f,1.0f);
        gl.TexCoord2f(0.5f,0.5f);
        gl.Vertex3f(0.0f,0.0f,topz);

        angle = 0.0f;
        texCoord = 0.0f;
        for(unsigned int topi=0;
            topi<numSegments;
            ++topi,angle+=angleDelta,texCoord+=texCoordDelta)
        {
            float c = cosf(angle);
            float s = sinf(angle);

            gl.TexCoord2f(c*0.5f+0.5f,s*0.5f+0.5f);
            gl.Vertex3f(c*r,s*r,topz);
        }

        gl.TexCoord2f(1.0f,0.5f);
        gl.Vertex3f(r,0.0f,topz);

        gl.End();
    }

    // cylinder bottom
    if (createBottom)
    {
        gl.Begin(GL_TRIANGLE_FAN);

        gl.Normal3f(0.0f,0.0f,-1.0f);
        gl.TexCoord2f(0.5f,0.5f);
        gl.Vertex3f(0.0f,0.0f,basez);

        angle = osg::PI*2.0f;
        texCoord = 1.0f;
        for(unsigned int bottomi=0;
            bottomi<numSegments;
            ++bottomi,angle-=angleDelta,texCoord-=texCoordDelta)
        {
            float c = cosf(angle);
            float s = sinf(angle);

            gl.TexCoord2f(c*0.5f+0.5f,s*0.5f+0.5f);
            gl.Vertex3f(c*r,s*r,basez);
        }

        gl.TexCoord2f(1.0f,0.5f);
        gl.Vertex3f(r,0.0f,basez);

        gl.End();
    }

    gl.PopMatrix();
}
Beispiel #4
0
void TrackBall::stop()
{
    m_rotation = rotation();
    m_paused = true;
}
Beispiel #5
0
	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);
					}

					RegularData3D::IndexType index(x, y, z);
					reg3d->getData(index) = score;

					if (no_best_poses_ > 0) // if saving best positions is enabled
					{
						if (best_poses_.size() < no_best_poses_ || score < best_poses_.rbegin()->first)
						{
							bool insert = 1;

							list<PoseList::iterator> to_be_deleted;

							for (PoseList::iterator it = best_poses_.begin(); it != best_poses_.end(); it++)
							{
								if (it->second.first.getDistance(position) < 3*resolution_)
								{
									if (score < it->first) // current position is better, so remove the other position
									{
										to_be_deleted.push_back(it);
									}
									else // current position is worse, so do not insert it
									{
										insert = false;
										break;
									}
								}
							}

							if (insert)
							{
								for (list < PoseList::iterator > ::iterator it = to_be_deleted.begin(); it != to_be_deleted.end(); it++)
								{
									best_poses_.erase(*it);
								}

								Vector3 rotation(best_angle_x, best_angle_y, best_angle_z);
								best_poses_.insert(make_pair(score, make_pair(position, rotation)));
							}
						}
						if (best_poses_.size() > no_best_poses_)
						{
							PoseList::iterator it = best_poses_.end();
							it--;
							best_poses_.erase(it);
						}
					}
				}
			}
		}

		scoring_function_->setLigand(*backup_ligand);
		return reg3d;
	}
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());

}
	/// Implements the plane optimal shadow camera setup algorithm
	void PlaneOptimalShadowCameraSetup::getShadowCamera (const SceneManager *sm, const Camera *cam, 
		const Viewport *vp, const Light *light, Camera *texCam, size_t iteration) const
	{
		// get the plane transformed by the parent node(s)
		// Also, make sure the plane is normalized
		Plane worldPlane = mPlane->_getDerivedPlane();
		worldPlane.normalise();

		// get camera's projection matrix
		Matrix4 camProjection = cam->getProjectionMatrix() * cam->getViewMatrix();

		// get the world points to constrain
		vector<Vector4>::type vhull;
		cam->forwardIntersect(worldPlane, &vhull);
		if (vhull.size() < 4)
			return;

		// make sure the last point is a finite point (not point at infinity)
        if (vhull[3].w == 0.0)
        {
		    int finiteIndex = -1;
		    for (uint loopIndex = 0; loopIndex < vhull.size(); loopIndex++)
		    {
			    if (vhull[loopIndex].w != 0.0)
                {
				    finiteIndex = loopIndex;
                    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);
	}
Beispiel #8
0
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()));

	gridXZAction = new QAction(tr("XZ Grid"), this);
    gridXZAction->setStatusTip(tr("Grid on XZ Plane"));
	//gridXZAction->setShortcut(tr("Ctrl+Y"));
    connect(gridXZAction, SIGNAL(triggered()), myVC, SLOT(gridXZ()));

	gridYZAction = new QAction(tr("YZ Grid"), this);
	gridYZAction->setStatusTip(tr("Grid on YZ Plane"));
	//gridOffAction->setShortcut(tr("Ctrl+Z"));
    connect(gridYZAction, SIGNAL(triggered()), myVC, SLOT(gridYZ()));

	gridRectAction = new QAction(tr("Rectangular"), this);
	gridRectAction->setStatusTip(tr("Retangular grid"));
	//gridOffAction->setShortcut(tr("Ctrl+Z"));
    connect(gridRectAction, SIGNAL(triggered()), myVC, SLOT(gridRect()));

	gridCircAction = new QAction(tr("Circular"), this);
	gridCircAction->setStatusTip(tr("Circular grid"));
	//gridOffAction->setShortcut(tr("Ctrl+Z"));
    connect(gridCircAction, SIGNAL(triggered()), myVC, SLOT(gridCirc()));

	// Standard View

	viewFrontAction = new QAction(tr("Front"), this);
	viewFrontAction->setStatusTip(tr("View From Front"));
    connect(viewFrontAction, SIGNAL(triggered()), myOCC, SLOT(viewFront()));

	viewBackAction = new QAction(tr("Back"), this);
	viewBackAction->setStatusTip(tr("View From Back"));
    connect(viewBackAction, SIGNAL(triggered()), myOCC, SLOT(viewBack()));

	viewTopAction = new QAction(tr("Top"), this);
	viewTopAction->setStatusTip(tr("View From Top"));
    connect(viewTopAction, SIGNAL(triggered()), myOCC, SLOT(viewTop()));

	viewBottomAction = new QAction(tr("Bottom"), this);
	viewBottomAction->setStatusTip(tr("View From Bottom"));
    connect(viewBottomAction, SIGNAL(triggered()), myOCC, SLOT(viewBottom()));

	viewLeftAction = new QAction(tr("Left"), this);
	viewLeftAction->setStatusTip(tr("View From Left"));
    connect(viewLeftAction, SIGNAL(triggered()), myOCC, SLOT(viewLeft()));

	viewRightAction = new QAction(tr("Right"), this);
	viewRightAction->setStatusTip(tr("View From Right"));
    connect(viewRightAction, SIGNAL(triggered()), myOCC, SLOT(viewRight()));

	viewAxoAction = new QAction(tr("&Axonometric Fit"), this);
	viewAxoAction->setStatusTip(tr("Axonometric view and fit all"));
    viewAxoAction->setShortcut(tr("Ctrl+A"));
    connect(viewAxoAction, SIGNAL(triggered()), myOCC, SLOT(viewAxo()));

	viewGridAction = new QAction(tr("Grid"), this);
	viewGridAction->setStatusTip(tr("View from grid"));
    connect(viewGridAction, SIGNAL(triggered()), myOCC, SLOT(viewGrid()));

	viewResetAction = new QAction(tr("Reset"), this);
	viewResetAction->setStatusTip(tr("Reset the view"));
    connect(viewResetAction, SIGNAL(triggered()), myOCC, SLOT(viewReset()));

	backgroundAction = new QAction( tr("&Background"), this );
	backgroundAction->setStatusTip(tr("Change the background colour"));
	connect(backgroundAction, SIGNAL(triggered()), myOCC, SLOT(background()));

	// The co-ordinates from the view
	connect( myOCC, SIGNAL(mouseMoved(V3d_Coordinate,V3d_Coordinate,V3d_Coordinate)),
		     this,   SLOT(xyzPosition(V3d_Coordinate,V3d_Coordinate,V3d_Coordinate)) );

	// Add a point from the view
	connect( myOCC, SIGNAL(pointClicked(V3d_Coordinate,V3d_Coordinate,V3d_Coordinate)),
		     this,   SLOT (addPoint    (V3d_Coordinate,V3d_Coordinate,V3d_Coordinate)) );

	connect( myOCC, SIGNAL(sendStatus(const QString)),
		     this,  SLOT  (statusMessage(const QString)) );

	// And the bottle example

	bottleAction = new QAction(tr("Load &Bottle"), this);
	bottleAction->setShortcut(tr("Ctrl+B"));
    bottleAction->setStatusTip(tr("Bottle sample."));
    connect(bottleAction, SIGNAL(triggered()), this, SLOT(bottle()));

}
Beispiel #9
0
void Custom6DOF::SubmitConstraints (dFloat timestep, int threadIndex)
{
	dMatrix matrix0;
	dMatrix matrix1;

	// calculate the position of the pivot point and the Jacobian direction vectors, in global space. 
	CalculateGlobalMatrix (m_localMatrix0, m_localMatrix1, matrix0, matrix1);

	// add the linear limits
	const dVector& p0 = matrix0.m_posit;
	const dVector& p1 = matrix1.m_posit;
	dVector dp (p0 - p1);

	for (int i = 0; i < 3; i ++) {
		if ((m_minLinearLimits[i] == 0.0f) && (m_maxLinearLimits[i] == 0.0f)) {
			NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0[i][0]);
			NewtonUserJointSetRowStiffness (m_joint, 1.0f);
		} else {
			// it is a limited linear dof, check if it pass the limits
			dFloat dist = dp % matrix1[i];
			if (dist > m_maxLinearLimits[i]) {
				dVector q1 (p1 + matrix1[i].Scale (m_maxLinearLimits[i]));

				// clamp the error, so the not too much energy is added when constraint violation occurs
				dFloat maxDist = (p0 - q1) % matrix1[i];
				if (maxDist > D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION) {
					q1 = p0 - matrix1[i].Scale(D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION);
				}

				NewtonUserJointAddLinearRow (m_joint, &p0[0], &q1[0], &matrix0[i][0]);
				NewtonUserJointSetRowStiffness (m_joint, 1.0f);
				// allow the object to return but not to kick going forward
				NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);

			} else if (dist < m_minLinearLimits[i]) {
				dVector q1 (p1 + matrix1[i].Scale (m_minLinearLimits[i]));

				// clamp the error, so the not too much energy is added when constraint violation occurs
				dFloat maxDist = (p0 - q1) % matrix1[i];
				if (maxDist < -D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION) {
					q1 = p0 - matrix1[i].Scale(-D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION);
				}

				NewtonUserJointAddLinearRow (m_joint, &p0[0], &q1[0], &matrix0[i][0]);
				NewtonUserJointSetRowStiffness (m_joint, 1.0f);
				// allow the object to return but not to kick going forward
				NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
			}
		}
	}

	dVector euler0;
	dVector euler1;
	dMatrix localMatrix (matrix0 * matrix1.Inverse());
	localMatrix.GetEulerAngles(euler0, euler1);

	AngularIntegration pitchStep0 (AngularIntegration (euler0.m_x) - m_pitch);
	AngularIntegration pitchStep1 (AngularIntegration (euler1.m_x) - m_pitch);
	if (dAbs (pitchStep0.m_angle) > dAbs (pitchStep1.m_angle)) {
		euler0 = euler1;
	}

	dVector euler (m_pitch.Update (euler0.m_x), m_yaw.Update (euler0.m_y), m_roll.Update (euler0.m_z), 0.0f);

//dTrace (("(%f %f %f) (%f %f %f)\n", m_pitch.m_angle * 180.0f / 3.141592f, m_yaw.m_angle * 180.0f / 3.141592f, m_roll.m_angle * 180.0f / 3.141592f,  euler0.m_x * 180.0f / 3.141592f, euler0.m_y * 180.0f / 3.141592f, euler0.m_z * 180.0f / 3.141592f));

	bool limitViolation = false;
	for (int i = 0; i < 3; i ++) {
		if (euler[i] < m_minAngularLimits[i]) {
			limitViolation = true;
			euler[i] = m_minAngularLimits[i];
		} else if (euler[i] > m_maxAngularLimits[i]) {
			limitViolation = true;
			euler[i] = m_maxAngularLimits[i];
		}
	}

	if (limitViolation) {
		//dMatrix pyr (dPitchMatrix(m_pitch.m_angle) * dYawMatrix(m_yaw.m_angle) * dRollMatrix(m_roll.m_angle));
		dMatrix p0y0r0 (dPitchMatrix(euler[0]) * dYawMatrix(euler[1]) * dRollMatrix(euler[2]));
		dMatrix baseMatrix (p0y0r0 * matrix1);
        dMatrix rotation (matrix0.Inverse() * baseMatrix);

        dQuaternion quat (rotation);
        if (quat.m_q0 > dFloat (0.99995f)) {
			//dVector p0 (matrix0[3] + baseMatrix[1].Scale (MIN_JOINT_PIN_LENGTH));
			//dVector p1 (matrix0[3] + baseMatrix[1].Scale (MIN_JOINT_PIN_LENGTH));
			//NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &baseMatrix[2][0]);
			//NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);

			//dVector q0 (matrix0[3] + baseMatrix[0].Scale (MIN_JOINT_PIN_LENGTH));
			//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &baseMatrix[1][0]);
			//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &baseMatrix[2][0]);

        } else {
            dMatrix basis (dGrammSchmidt (dVector (quat.m_q1, quat.m_q2, quat.m_q3, 0.0f)));

			dVector p0 (matrix0[3] + basis[1].Scale (MIN_JOINT_PIN_LENGTH));
			dVector p1 (matrix0[3] + rotation.RotateVector(basis[1].Scale (MIN_JOINT_PIN_LENGTH)));
			NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &basis[2][0]);
			NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);

			//dVector q0 (matrix0[3] + basis[0].Scale (MIN_JOINT_PIN_LENGTH));
			//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &basis[1][0]);
			//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &basis[2][0]);
        }
	}
}
Beispiel #10
0
// The main mex-function
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
    // Check if right number of arguments
    if( nrhs < 3 || nrhs > 5 )
    {
        mexPrintf("opengv: Not an acceptable number of arguments\n");
        mexPrintf("Usage:  X = opengv( method, data1, data2 )\n");
        mexPrintf("Or:     X = opengv( method, indices, data1, data2 )\n");
        mexPrintf("Or:     X = opengv( method, indices, data1, data2, prior )\n");
        return;
    }

    // Get the method
    if( mxGetM(prhs[0]) != 1 )
    {
        mexPrintf("opengv: Bad input to mex function opengv\n");
        mexPrintf("Usage:  X = opengv( method, data1, data2 )\n");
        mexPrintf("Or:     X = opengv( method, indices, data1, data2 )\n");
        mexPrintf("Or:     X = opengv( method, indices, data1, data2, prior )\n");
        mexPrintf("Hint:   Method must be a string\n");
        return;
    }

    // Now get the string and find the caseNumber
    mwSize strlen = (mwSize) mxGetN(prhs[0]) + 1;
    char * method = (char *) malloc(strlen);
    mxGetString(prhs[0], method, strlen);
    int caseNumber = findCase(method, (int) mxGetN(prhs[0]));

    // Return if method not found
    if( caseNumber < 0 )
    {
        mexPrintf("opengv: Unknown method\n");
        printCases();
        return;
    }

    // Characterize the type of the call
    int callCharacter = -1;
    const mxArray *data1;
    const mxArray *data2;
    const mwSize *data1dim;
    const mwSize *data2dim;

    if( nrhs == 3 ) // X = opengv( method, data1, data2 )
    {
        // Check the input
        data1 = prhs[1];
        data2 = prhs[2];

        // Check the dimensions of the arguments
        int ndimensions1 = mxGetNumberOfDimensions(data1);
        int ndimensions2 = mxGetNumberOfDimensions(data2);
        data1dim = mxGetDimensions(data1);
        data2dim = mxGetDimensions(data2);

        // Now check them
        if( ndimensions1 != 2 || ndimensions2 != 2 ||
                (data1dim[0] != 3 && data1dim[0] != 6) ||
                (data2dim[0] != 3 && data2dim[0] != 6) ||
                data1dim[1] != data2dim[1] ||
                data1dim[1] < 1 || data2dim[1] < 1 )
        {
            mexPrintf("opengv: Bad input to mex function\n");
            mexPrintf("Assuming signature: X = opengv( method, data1, data2 )\n");
            mexPrintf("Inputs data1 and data2 must have size (3,n) or (6,n),\n");
            mexPrintf("with an equal number of columns\n");
            return;
        }

        callCharacter = 0;
    }
    if( nrhs == 4 )
    {
        // X = opengv( method, indices, data1, data2 )

        // Check the input
        data1 = prhs[2];
        data2 = prhs[3];

        // Check the dimensions of the arguments
        int ndimensions1 = mxGetNumberOfDimensions(data1);
        int ndimensions2 = mxGetNumberOfDimensions(data2);
        int ndimensions3 = mxGetNumberOfDimensions(prhs[1]);
        data1dim = mxGetDimensions(data1);
        data2dim = mxGetDimensions(data2);
        const mwSize *indicesDim = mxGetDimensions(prhs[1]);

        // Now check them
        if( ndimensions1 != 2 || ndimensions2 != 2 || ndimensions3 != 2 ||
                (data1dim[0] != 3 && data1dim[0] != 6) ||
                (data2dim[0] != 3 && data2dim[0] != 6) ||
                indicesDim[0] != 1 ||
                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 )\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");
            return;
        }

        callCharacter = 1;
    }
    if(nrhs == 5)
    {
        // X = opengv( method, indices, data1, data2, prior )

        // Check the input
        data1 = prhs[2];
        data2 = prhs[3];

        // Check the dimensions of the arguments
        int ndimensions1 = mxGetNumberOfDimensions(data1);
        int ndimensions2 = mxGetNumberOfDimensions(data2);
        int ndimensions3 = mxGetNumberOfDimensions(prhs[1]);
        int ndimensions4 = mxGetNumberOfDimensions(prhs[4]);
        data1dim = mxGetDimensions(data1);
        data2dim = mxGetDimensions(data2);
        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];
            rotation(0,2) = ptr[6];
            rotation(1,2) = ptr[7];
            rotation(2,2) = ptr[8];
        }
        if( priorDim[1] == 4 )
        {
            translationPrior = 1;
            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];
            rotation(0,2) = ptr[6];
            rotation(1,2) = ptr[7];
            rotation(2,2) = ptr[8];
            translation[0] = ptr[9];
            translation[1] = ptr[10];
            translation[2] = ptr[11];
        }
    }

    if( caseNumber >= absCentralFirst && caseNumber <= absCentralLast )
    {
        //central absolute case
        if( data1dim[0] != 3 || data2dim[0] != 3 )
        {
            mexPrintf("opengv: Bad input to mex function opengv\n");
            mexPrintf("Assuming method: ");
            mexPrintf(methods[caseNumber]);
            mexPrintf("\n");
            mexPrintf("Inputs data1 and data2 must have size (3,n) for a central ");
            mexPrintf("absolute method\n");
            return;
        }

        absoluteAdapter = new opengv::absolute_pose::MACentralAbsolute(
            (double*) mxGetData(data1),
            (double*) mxGetData(data2),
            data1dim[1],
            data2dim[1] );

        if( translationPrior == 1 )
            absoluteAdapter->sett(translation);
        if( rotationPrior == 1 )
            absoluteAdapter->setR(rotation);
    }

    if(caseNumber >= absNoncentralFirst && caseNumber <= absNoncentralLast )
    {
        //non-central absolute case
        if( data1dim[0] != 3 || data2dim[0] != 6 )
        {
            mexPrintf("opengv: Bad input to mex function opengv\n");
            mexPrintf("Assuming method: ");
            mexPrintf(methods[caseNumber]);
            mexPrintf("\n");
            mexPrintf("Inputs data1 and data2 must have sizes (3,n) and (6,n) for ");
            mexPrintf("a noncentral absolute method\n");
            return;
        }

        absoluteAdapter = new opengv::absolute_pose::MANoncentralAbsolute(
            (double*) mxGetData(data1),
            (double*) mxGetData(data2),
            data1dim[1],
            data2dim[1] );

        if( translationPrior == 1 )
            absoluteAdapter->sett(translation);
        if( rotationPrior == 1 )
            absoluteAdapter->setR(rotation);
    }
    if(caseNumber >= relCentralFirst && caseNumber <= relCentralLast )
    {
        //central relative case
        if( data1dim[0] != 3 || data2dim[0] != 3 )
        {
            mexPrintf("opengv: Bad input to mex function opengv\n");
            mexPrintf("Assuming method: ");
            mexPrintf(methods[caseNumber]);
            mexPrintf("\n");
            mexPrintf("Inputs data1 and data2 must have size (3,n) for a central ");
            mexPrintf("relative method\n");
            return;
        }

        relativeAdapter = new opengv::relative_pose::MACentralRelative(
            (double*) mxGetData(data1),
            (double*) mxGetData(data2),
            data1dim[1],
            data2dim[1] );

        if( translationPrior == 1 )
            relativeAdapter->sett12(translation);
        if( rotationPrior == 1 )
            relativeAdapter->setR12(rotation);
    }

    if(caseNumber >= relNoncentralFirst && caseNumber <= relNoncentralLast )
    {
        //noncentral relative case
        if( data1dim[0] != 6 || data2dim[0] != 6 )
        {
            mexPrintf("opengv: Bad input to mex function opengv\n");
            mexPrintf("Assuming method: ");
            mexPrintf(methods[caseNumber]);
            mexPrintf("\n");
            mexPrintf("Inputs data1 and data2 must have size (6,n) for a ");
            mexPrintf("noncentral relative method\n");
            return;
        }

        relativeAdapter = new opengv::relative_pose::MANoncentralRelative(
            (double*) mxGetData(data1),
            (double*) mxGetData(data2),
            data1dim[1],
            data2dim[1] );

        if( translationPrior == 1 )
            relativeAdapter->sett12(translation);
        if( rotationPrior == 1 )
            relativeAdapter->setR12(rotation);
    }

    if(caseNumber >= pointCloudFirst && caseNumber <= pointCloudLast )
    {
        //point-cloud case
        if( data1dim[0] != 3 || data2dim[0] != 3 )
        {
            mexPrintf("opengv: Bad input to mex function opengv\n");
            mexPrintf("Assuming method: ");
            mexPrintf(methods[caseNumber]);
            mexPrintf("\n");
            mexPrintf("Inputs data1 and data2 must have size (3,n) for a ");
            mexPrintf("point-cloud method\n");
            return;
        }

        pointCloudAdapter = new opengv::point_cloud::MAPointCloud(
            (double*) mxGetData(data1),
            (double*) mxGetData(data2),
            data1dim[1],
            data2dim[1] );

        if( translationPrior == 1 )
            pointCloudAdapter->sett12(translation);
        if( rotationPrior == 1 )
            pointCloudAdapter->setR12(rotation);
    }

    //check if a return argument is needed, otherwise we won't start computing
    if( nlhs != 1 )
    {
        if( nlhs > 1 )
            mexPrintf("opengv: Returns one parameter only\n");
        return;
    }

    //create the indices array (todo: check if there is a smarter way for doing this)
    std::vector<int> indices;
    int useIndices = 0;
    if( callCharacter > 0 )
    {
        useIndices = 1;
        const mwSize *indicesDim = mxGetDimensions(prhs[1]);
        int numberOfIndices = indicesDim[1];
        indices.reserve(numberOfIndices);
        double * mxIndices = (double*) mxGetData(prhs[1]);
        for( int i = 0; i < numberOfIndices; i++ )
            indices.push_back(floor(mxIndices[i]+0.01)-1);
    }

    Method methodEnum = static_cast<Method>(caseNumber);
    if( caseNumber != (int) methodEnum )
    {
        mexPrintf("opengv: This method is not yet implemented!\n");
        return;
    }

    // Finally, call the respective algorithm
    switch (methodEnum)
    {
    case P2P:
    {
        opengv::translation_t temp;
        if(useIndices)
            temp = opengv::absolute_pose::p2p(*absoluteAdapter,indices);
        else
            temp = opengv::absolute_pose::p2p(*absoluteAdapter);
        int dims[2];
        dims[0] = 3;
        dims[1] = 1;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetData(plhs[0]), temp.data(), 3*sizeof(double));
        break;
    }
    case P3P_KNEIP:
    {
        opengv::transformations_t temp;
        if(useIndices)
            temp = opengv::absolute_pose::p3p_kneip(*absoluteAdapter,indices);
        else
            temp = opengv::absolute_pose::p3p_kneip(*absoluteAdapter);
        int dims[3];
        dims[0] = 3;
        dims[1] = 4;
        dims[2] = temp.size();
        plhs[0] = mxCreateNumericArray(3, dims, mxDOUBLE_CLASS, mxREAL);
        for( int i = 0; i < temp.size(); i++ )
        {
            void * targetAddress = ((char*) mxGetData(plhs[0])) + i*12*sizeof(double);
            memcpy(targetAddress, temp[i].data(), 12*sizeof(double));
        }
        break;
    }
    case P3P_GAO:
    {
        opengv::transformations_t temp;
        if(useIndices)
            temp = opengv::absolute_pose::p3p_gao(*absoluteAdapter,indices);
        else
            temp = opengv::absolute_pose::p3p_gao(*absoluteAdapter);
        int dims[3];
        dims[0] = 3;
        dims[1] = 4;
        dims[2] = temp.size();
        plhs[0] = mxCreateNumericArray(3, dims, mxDOUBLE_CLASS, mxREAL);
        for( int i = 0; i < temp.size(); i++ )
        {
            void * targetAddress = ((char*) mxGetData(plhs[0])) + i*12*sizeof(double);
            memcpy(targetAddress, temp[i].data(), 12*sizeof(double));
        }
        break;
    }
    case EPNP:
    {
        opengv::transformation_t temp;
        if(useIndices)
            temp = opengv::absolute_pose::epnp(*absoluteAdapter,indices);
        else
            temp = opengv::absolute_pose::epnp(*absoluteAdapter);
        int dims[2];
        dims[0] = 3;
        dims[1] = 4;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetData(plhs[0]), temp.data(), 12*sizeof(double));
        break;
    }
    case P3P_KNEIP_RANSAC:
    {
        absRansacPtr problem;
        if(useIndices)
            problem = absRansacPtr( new absRansac( *absoluteAdapter, absRansac::KNEIP, indices ) );
        else
            problem = absRansacPtr( new absRansac( *absoluteAdapter, absRansac::KNEIP ) );
        opengv::sac::Ransac<absRansac> ransac;
        ransac.sac_model_ = problem;
        ransac.threshold_ = 1.0 - cos(atan(sqrt(2.0)*0.5/800.0));
        ransac.max_iterations_ = 50;
        ransac.computeModel();
        int dims[2];
        dims[0] = 3;
        dims[1] = 4;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetData(plhs[0]), ransac.model_coefficients_.data(), 12*sizeof(double));
        break;
    }
    case P3P_GAO_RANSAC:
    {
        absRansacPtr problem;
        if(useIndices)
            problem = absRansacPtr( new absRansac( *absoluteAdapter, absRansac::GAO, indices ) );
        else
            problem = absRansacPtr( new absRansac( *absoluteAdapter, absRansac::GAO ) );
        opengv::sac::Ransac<absRansac> ransac;
        ransac.sac_model_ = problem;
        ransac.threshold_ = 1.0 - cos(atan(sqrt(2.0)*0.5/800.0));
        ransac.max_iterations_ = 50;
        ransac.computeModel();
        int dims[2];
        dims[0] = 3;
        dims[1] = 4;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetData(plhs[0]), ransac.model_coefficients_.data(), 12*sizeof(double));
        break;
    }
    case EPNP_RANSAC:
    {
        absRansacPtr problem;
        if(useIndices)
            problem = absRansacPtr( new absRansac( *absoluteAdapter, absRansac::EPNP, indices ) );
        else
            problem = absRansacPtr( new absRansac( *absoluteAdapter, absRansac::EPNP ) );
        opengv::sac::Ransac<absRansac> ransac;
        ransac.sac_model_ = problem;
        ransac.threshold_ = 1.0 - cos(atan(sqrt(2.0)*0.5/800.0));
        ransac.max_iterations_ = 50;
        ransac.computeModel();
        int dims[2];
        dims[0] = 3;
        dims[1] = 4;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetData(plhs[0]), ransac.model_coefficients_.data(), 12*sizeof(double));
        break;
    }
    case ABS_NONLIN_CENTRAL:
    {
        opengv::transformation_t temp;
        if(useIndices)
            temp = opengv::absolute_pose::optimize_nonlinear(*absoluteAdapter,indices);
        else
            temp = opengv::absolute_pose::optimize_nonlinear(*absoluteAdapter);
        int dims[2];
        dims[0] = 3;
        dims[1] = 4;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetData(plhs[0]), temp.data(), 12*sizeof(double));
        break;
    }
    case GP3P:
    {
        opengv::transformations_t temp;
        if(useIndices)
            temp = opengv::absolute_pose::gp3p(*absoluteAdapter,indices);
        else
            temp = opengv::absolute_pose::gp3p(*absoluteAdapter);
        int dims[3];
        dims[0] = 3;
        dims[1] = 4;
        dims[2] = temp.size();
        plhs[0] = mxCreateNumericArray(3, dims, mxDOUBLE_CLASS, mxREAL);
        for( int i = 0; i < temp.size(); i++ )
        {
            void * targetAddress = ((char*) mxGetData(plhs[0])) + i*12*sizeof(double);
            memcpy(targetAddress, temp[i].data(), 12*sizeof(double));
        }
        break;
    }
    case GP3P_RANSAC:
    {
        absRansacPtr problem;
        if(useIndices)
            problem = absRansacPtr( new absRansac( *absoluteAdapter, absRansac::GP3P, indices ) );
        else
            problem = absRansacPtr( new absRansac( *absoluteAdapter, absRansac::GP3P ) );
        opengv::sac::Ransac<absRansac> ransac;
        ransac.sac_model_ = problem;
        ransac.threshold_ = 1.0 - cos(atan(sqrt(2.0)*0.5/800.0));
        ransac.max_iterations_ = 50;
        ransac.computeModel();
        int dims[2];
        dims[0] = 3;
        dims[1] = 4;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetData(plhs[0]), ransac.model_coefficients_.data(), 12*sizeof(double));
        break;
    }
    case GPNP:
    {
        opengv::transformation_t temp;
        if(useIndices)
            temp = opengv::absolute_pose::gpnp(*absoluteAdapter,indices);
        else
            temp = opengv::absolute_pose::gpnp(*absoluteAdapter);
        int dims[2];
        dims[0] = 3;
        dims[1] = 4;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetData(plhs[0]), temp.data(), 12*sizeof(double));
        break;
    }
    case ABS_NONLIN_NONCENTRAL:
    {
        opengv::transformation_t temp;
        if(useIndices)
            temp = opengv::absolute_pose::optimize_nonlinear(*absoluteAdapter,indices);
        else
            temp = opengv::absolute_pose::optimize_nonlinear(*absoluteAdapter);
        int dims[2];
        dims[0] = 3;
        dims[1] = 4;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetData(plhs[0]), temp.data(), 12*sizeof(double));
        break;
    }
    case TWOPT:
    {
        opengv::translation_t temp;
        if(useIndices)
            temp = opengv::relative_pose::twopt(*relativeAdapter,false,indices);
        else
            temp = opengv::relative_pose::twopt(*relativeAdapter,false);
        int dims[2];
        dims[0] = 3;
        dims[1] = 1;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetData(plhs[0]), temp.data(), 3*sizeof(double));
        break;
    }
    case TWOPT_ROTATIONONLY:
    {
        opengv::rotation_t temp;
        if(useIndices)
            temp = opengv::relative_pose::twopt_rotationOnly(*relativeAdapter,indices);
        else
            temp = opengv::relative_pose::twopt_rotationOnly(*relativeAdapter);
        int dims[2];
        dims[0] = 3;
        dims[1] = 3;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetData(plhs[0]), temp.data(), 9*sizeof(double));
        break;
    }
    case ROTATIONONLY:
    {
        opengv::rotation_t temp;
        if(useIndices)
            temp = opengv::relative_pose::rotationOnly(*relativeAdapter,indices);
        else
            temp = opengv::relative_pose::rotationOnly(*relativeAdapter);
        int dims[2];
        dims[0] = 3;
        dims[1] = 3;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetData(plhs[0]), temp.data(), 9*sizeof(double));
        break;
    }
    case FIVEPT_STEWENIUS:
    {
        opengv::complexEssentials_t temp2;
        if(useIndices)
            temp2 = opengv::relative_pose::fivept_stewenius(*relativeAdapter,indices);
        else
            temp2 = opengv::relative_pose::fivept_stewenius(*relativeAdapter);
        opengv::essentials_t temp;
        for(size_t i = 0; i < temp2.size(); i++)
        {
            opengv::essential_t essentialMatrix;
            for(size_t r = 0; r < 3; r++)
            {
                for(size_t c = 0; c < 3; c++)
                    essentialMatrix(r,c) = temp2[i](r,c).real();
            }
            temp.push_back(essentialMatrix);
        }
        int dims[3];
        dims[0] = 3;
        dims[1] = 3;
        dims[2] = temp.size();
        plhs[0] = mxCreateNumericArray(3, dims, mxDOUBLE_CLASS, mxREAL);
        for( int i = 0; i < temp.size(); i++ )
        {
            void * targetAddress = ((char*) mxGetData(plhs[0])) + i*9*sizeof(double);
            memcpy(targetAddress, temp[i].data(), 9*sizeof(double));
        }
        break;
    }
    case FIVEPT_NISTER:
    {
        opengv::essentials_t temp;
        if(useIndices)
            temp = opengv::relative_pose::fivept_nister(*relativeAdapter,indices);
        else
            temp = opengv::relative_pose::fivept_nister(*relativeAdapter);
        int dims[3];
        dims[0] = 3;
        dims[1] = 3;
        dims[2] = temp.size();
        plhs[0] = mxCreateNumericArray(3, dims, mxDOUBLE_CLASS, mxREAL);
        for( int i = 0; i < temp.size(); i++ )
        {
            void * targetAddress = ((char*) mxGetData(plhs[0])) + i*9*sizeof(double);
            memcpy(targetAddress, temp[i].data(), 9*sizeof(double));
        }
        break;
    }
    case FIVEPT_KNEIP:
    {
        opengv::rotations_t temp;
        if(useIndices)
            temp = opengv::relative_pose::fivept_kneip(*relativeAdapter,indices);
        else
        {
            mexPrintf("opengv: Bad input to mex function opengv\n");
            mexPrintf("Assuming method: ");
            mexPrintf(methods[caseNumber]);
            mexPrintf("\n");
            mexPrintf("You must provide an indices vector\n");
            break;
        }
        int dims[3];
        dims[0] = 3;
        dims[1] = 3;
        dims[2] = temp.size();
        plhs[0] = mxCreateNumericArray(3, dims, mxDOUBLE_CLASS, mxREAL);
        for( int i = 0; i < temp.size(); i++ )
        {
            void * targetAddress = ((char*) mxGetData(plhs[0])) + i*9*sizeof(double);
            memcpy(targetAddress, temp[i].data(), 9*sizeof(double));
        }
        break;
    }
    case SEVENPT:
    {
        opengv::essentials_t temp;
        if(useIndices)
            temp = opengv::relative_pose::sevenpt(*relativeAdapter,indices);
        else
            temp = opengv::relative_pose::sevenpt(*relativeAdapter);
        int dims[3];
        dims[0] = 3;
        dims[1] = 3;
        dims[2] = temp.size();
        plhs[0] = mxCreateNumericArray(3, dims, mxDOUBLE_CLASS, mxREAL);
        for( int i = 0; i < temp.size(); i++ )
        {
            void * targetAddress = ((char*) mxGetData(plhs[0])) + i*9*sizeof(double);
            memcpy(targetAddress, temp[i].data(), 9*sizeof(double));
        }
        break;
    }
    case EIGHTPT:
    {
        opengv::essential_t temp;
        if(useIndices)
            temp = opengv::relative_pose::eightpt(*relativeAdapter,indices);
        else
            temp = opengv::relative_pose::eightpt(*relativeAdapter);
        int dims[2];
        dims[0] = 3;
        dims[1] = 3;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetData(plhs[0]), temp.data(), 9*sizeof(double));
        break;
    }
    case EIGENSOLVER:
    {
        opengv::rotation_t temp;
        if(useIndices)
            temp = opengv::relative_pose::eigensolver(*relativeAdapter,indices);
        else
            temp = opengv::relative_pose::eigensolver(*relativeAdapter);
        int dims[2];
        dims[0] = 3;
        dims[1] = 3;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetData(plhs[0]), temp.data(), 9*sizeof(double));
        break;
    }
    case ROTATIONONLY_RANSAC:
    {
        rotRansacPtr problem;
        if(useIndices)
            problem = rotRansacPtr( new rotRansac( *relativeAdapter, indices ) );
        else
            problem = rotRansacPtr( new rotRansac( *relativeAdapter ) );
        opengv::sac::Ransac<rotRansac> ransac;
        ransac.sac_model_ = problem;
        ransac.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0)));
        ransac.max_iterations_ = 50;
        ransac.computeModel();
        int dims[2];
        dims[0] = 3;
        dims[1] = 3;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetData(plhs[0]), ransac.model_coefficients_.data(), 9*sizeof(double));
        break;
    }
    case FIVEPT_STEWENIUS_RANSAC:
    {
        relRansacPtr problem;
        if(useIndices)
            problem = relRansacPtr( new relRansac( *relativeAdapter, relRansac::STEWENIUS, indices ) );
        else
            problem = relRansacPtr( new relRansac( *relativeAdapter, relRansac::STEWENIUS ) );
        opengv::sac::Ransac<relRansac> ransac;
        ransac.sac_model_ = problem;
        ransac.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0)));
        ransac.max_iterations_ = 50;
        ransac.computeModel();

        opengv::transformation_t optimizedModel;
        problem->optimizeModelCoefficients(ransac.inliers_,ransac.model_coefficients_,optimizedModel);

        int dims[2];
        dims[0] = 3;
        dims[1] = 4;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        //memcpy(mxGetData(plhs[0]), ransac.model_coefficients_.data(), 12*sizeof(double));
        memcpy(mxGetData(plhs[0]), optimizedModel.data(), 12*sizeof(double));
        break;
    }
    case FIVEPT_NISTER_RANSAC:
    {
        relRansacPtr problem;
        if(useIndices)
            problem = relRansacPtr( new relRansac( *relativeAdapter, relRansac::NISTER, indices ) );
        else
            problem = relRansacPtr( new relRansac( *relativeAdapter, relRansac::NISTER ) );
        opengv::sac::Ransac<relRansac> ransac;
        ransac.sac_model_ = problem;
        ransac.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0)));
        ransac.max_iterations_ = 50;
        ransac.computeModel();
        int dims[2];
        dims[0] = 3;
        dims[1] = 4;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetData(plhs[0]), ransac.model_coefficients_.data(), 12*sizeof(double));
        break;
    }
    case SEVENPT_RANSAC:
    {
        relRansacPtr problem;
        if(useIndices)
            problem = relRansacPtr( new relRansac( *relativeAdapter, relRansac::SEVENPT, indices ) );
        else
            problem = relRansacPtr( new relRansac( *relativeAdapter, relRansac::SEVENPT ) );
        opengv::sac::Ransac<relRansac> ransac;
        ransac.sac_model_ = problem;
        ransac.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0)));
        ransac.max_iterations_ = 50;
        ransac.computeModel();
        int dims[2];
        dims[0] = 3;
        dims[1] = 4;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetData(plhs[0]), ransac.model_coefficients_.data(), 12*sizeof(double));
        break;
    }
    case EIGHTPT_RANSAC:
    {
        relRansacPtr problem;
        if(useIndices)
            problem = relRansacPtr( new relRansac( *relativeAdapter, relRansac::EIGHTPT, indices ) );
        else
            problem = relRansacPtr( new relRansac( *relativeAdapter, relRansac::EIGHTPT ) );
        opengv::sac::Ransac<relRansac> ransac;
        ransac.sac_model_ = problem;
        ransac.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0)));
        ransac.max_iterations_ = 50;
        ransac.computeModel();
        int dims[2];
        dims[0] = 3;
        dims[1] = 4;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetData(plhs[0]), ransac.model_coefficients_.data(), 12*sizeof(double));
        break;
    }
    case EIGENSOLVER_RANSAC:
    {
        eigRansacPtr problem;
        if(useIndices)
            problem = eigRansacPtr( new eigRansac( *relativeAdapter, 10, indices ) );
        else
            problem = eigRansacPtr( new eigRansac( *relativeAdapter, 10 ) );
        opengv::sac::Ransac<eigRansac> ransac;
        ransac.sac_model_ = problem;
        ransac.threshold_ = 1.0;
        ransac.max_iterations_ = 50;
        ransac.computeModel();
        opengv::transformation_t temp;
        temp.block<3,3>(0,0) = ransac.model_coefficients_.rotation;
        temp.block<3,1>(0,3) = ransac.model_coefficients_.translation;
        int dims[2];
        dims[0] = 3;
        dims[1] = 4;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetData(plhs[0]), temp.data(), 12*sizeof(double));
        break;
    }
    case REL_NONLIN_CENTRAL:
    {
        opengv::transformation_t temp;
        if(useIndices)
            temp = opengv::relative_pose::optimize_nonlinear(*relativeAdapter,indices);
        else
            temp = opengv::relative_pose::optimize_nonlinear(*relativeAdapter);
        int dims[2];
        dims[0] = 3;
        dims[1] = 4;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetData(plhs[0]), temp.data(), 12*sizeof(double));
        break;
    }
    case SIXPT:
    {
        opengv::rotations_t temp;
        if(useIndices)
            temp = opengv::relative_pose::sixpt(*relativeAdapter,indices);
        else
            temp = opengv::relative_pose::sixpt(*relativeAdapter);
        int dims[3];
        dims[0] = 3;
        dims[1] = 3;
        dims[2] = temp.size();
        plhs[0] = mxCreateNumericArray(3, dims, mxDOUBLE_CLASS, mxREAL);
        for( int i = 0; i < temp.size(); i++ )
        {
            void * targetAddress = ((char*) mxGetData(plhs[0])) + i*9*sizeof(double);
            memcpy(targetAddress, temp[i].data(), 9*sizeof(double));
        }
        break;
    }
    case SEVENTEENPT:
    {
        opengv::transformation_t temp;
        if(useIndices)
            temp = opengv::relative_pose::seventeenpt(*relativeAdapter,indices);
        else
            temp = opengv::relative_pose::seventeenpt(*relativeAdapter);
        int dims[2];
        dims[0] = 3;
        dims[1] = 4;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetData(plhs[0]), temp.data(), 12*sizeof(double));
        break;
    }
    case GE:
    {
        opengv::rotation_t temp;
        opengv::geOutput_t output;
        output.rotation = relativeAdapter->getR12();
        if(useIndices)
            temp = opengv::relative_pose::ge(*relativeAdapter,indices,output);
        else
            temp = opengv::relative_pose::ge(*relativeAdapter,output);
        int dims[2];
        dims[0] = 3;
        dims[1] = 3;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetData(plhs[0]),temp.data(), 9*sizeof(double));
        break;
    }
    case SIXPT_RANSAC:
    {
        nrelRansacPtr problem;
        if(useIndices)
            problem = nrelRansacPtr( new nrelRansac( *relativeAdapter, nrelRansac::SIXPT, indices ) );
        else
            problem = nrelRansacPtr( new nrelRansac( *relativeAdapter, nrelRansac::SIXPT ) );
        opengv::sac::Ransac<nrelRansac> ransac;
        ransac.sac_model_ = problem;
        ransac.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0)));
        ransac.max_iterations_ = 50;
        ransac.computeModel();
        int dims[2];
        dims[0] = 3;
        dims[1] = 4;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetData(plhs[0]), ransac.model_coefficients_.data(), 12*sizeof(double));
        break;
    }
    case SEVENTEENPT_RANSAC:
    {
        nrelRansacPtr problem;
        if(useIndices)
            problem = nrelRansacPtr( new nrelRansac( *relativeAdapter, nrelRansac::SEVENTEENPT, indices ) );
        else
            problem = nrelRansacPtr( new nrelRansac( *relativeAdapter, nrelRansac::SEVENTEENPT ) );
        opengv::sac::Ransac<nrelRansac> ransac;
        ransac.sac_model_ = problem;
        ransac.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0)));
        ransac.max_iterations_ = 50;
        ransac.computeModel();
        int dims[2];
        dims[0] = 3;
        dims[1] = 4;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetData(plhs[0]), ransac.model_coefficients_.data(), 12*sizeof(double));
        break;
    }
    case GE_RANSAC:
    {
        nrelRansacPtr problem;
        if(useIndices)
            problem = nrelRansacPtr( new nrelRansac( *relativeAdapter, nrelRansac::GE, indices ) );
        else
            problem = nrelRansacPtr( new nrelRansac( *relativeAdapter, nrelRansac::GE ) );
        opengv::sac::Ransac<nrelRansac> ransac;
        ransac.sac_model_ = problem;
        ransac.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0)));
        ransac.max_iterations_ = 50;
        ransac.computeModel();
        int dims[2];
        dims[0] = 3;
        dims[1] = 4;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetData(plhs[0]), ransac.model_coefficients_.data(), 12*sizeof(double));
        break;
    }
    case REL_NONLIN_NONCENTRAL:
    {
        opengv::transformation_t temp;
        if(useIndices)
            temp = opengv::relative_pose::optimize_nonlinear(*relativeAdapter,indices);
        else
            temp = opengv::relative_pose::optimize_nonlinear(*relativeAdapter);
        int dims[2];
        dims[0] = 3;
        dims[1] = 4;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetData(plhs[0]), temp.data(), 12*sizeof(double));
        break;
    }
    case THREEPT_ARUN:
    {
        opengv::transformation_t temp;
        if(useIndices)
            temp = opengv::point_cloud::threept_arun(*pointCloudAdapter,indices);
        else
            temp = opengv::point_cloud::threept_arun(*pointCloudAdapter);
        int dims[2];
        dims[0] = 3;
        dims[1] = 4;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetData(plhs[0]), temp.data(), 12*sizeof(double));
        break;
    }
    case THREEPT_ARUN_RANSAC:
    {
        ptRansacPtr problem;
        if(useIndices)
            problem = ptRansacPtr( new ptRansac( *pointCloudAdapter, indices ) );
        else
            problem = ptRansacPtr( new ptRansac( *pointCloudAdapter ) );
        opengv::sac::Ransac<ptRansac> ransac;
        ransac.sac_model_ = problem;
        ransac.threshold_ = 0.1;
        ransac.max_iterations_ = 50;
        ransac.computeModel();
        int dims[2];
        dims[0] = 3;
        dims[1] = 4;
        plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetData(plhs[0]), ransac.model_coefficients_.data(), 12*sizeof(double));
        break;
    }
    default: //-1
    {
        // impossible
        break;
    }
    }
}
Beispiel #11
0
void SixenseManager::update(float deltaTime) {
#ifdef HAVE_SIXENSE
    Hand* hand = DependencyManager::get<AvatarManager>()->getMyAvatar()->getHand();
    if (_isInitialized && _isEnabled) {
#ifdef __APPLE__
        SixenseBaseFunction sixenseGetNumActiveControllers =
        (SixenseBaseFunction) _sixenseLibrary->resolve("sixenseGetNumActiveControllers");
#endif
        
        if (sixenseGetNumActiveControllers() == 0) {
            _hydrasConnected = false;
            return;
        }
        
        PerformanceTimer perfTimer("sixense");
        if (!_hydrasConnected) {
            _hydrasConnected = true;
            UserActivityLogger::getInstance().connectedDevice("spatial_controller", "hydra");
        }
        
#ifdef __APPLE__
        SixenseBaseFunction sixenseGetMaxControllers =
        (SixenseBaseFunction) _sixenseLibrary->resolve("sixenseGetMaxControllers");
#endif
        
        int maxControllers = sixenseGetMaxControllers();
        
        // we only support two controllers
        sixenseControllerData controllers[2];
        
#ifdef __APPLE__
        SixenseTakeIntFunction sixenseIsControllerEnabled =
        (SixenseTakeIntFunction) _sixenseLibrary->resolve("sixenseIsControllerEnabled");
        
        SixenseTakeIntAndSixenseControllerData sixenseGetNewestData =
        (SixenseTakeIntAndSixenseControllerData) _sixenseLibrary->resolve("sixenseGetNewestData");
#endif
        int numControllersAtBase = 0;
        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);
                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
}
Beispiel #12
0
int Node::getNodeRotationAngle () const
{
    return static_cast<int>(rotation());
}
int main(int argc, char** argv) {
    
    double target_x = 0.0;
    double target_y = 0.0;
    double target_z = 0.0;
    
    
    ros::init(argc, argv, "arm_track");
    ros::NodeHandle n;
    ros::Rate r(27);
    ros::Publisher vel_pub = n.advertise<sensor_msgs::JointState> (
		    "/schunk/target_vel_safe/joint_states", 1);

    tf::TransformListener transformer;
    geometry_msgs::PoseStamped target_pose;
    
    target_pose.pose.position.x = target_x;
    target_pose.pose.position.y = target_y;
    target_pose.pose.position.z = target_z;
    
    target_pose.pose.orientation.x = 0;
    target_pose.pose.orientation.y = 0;
    target_pose.pose.orientation.z = 0;
    target_pose.pose.orientation.w = 1;
    
    target_pose.header.frame_id = "/head";
//     target_pose.header.frame_id = "/schunk/position/PAM112_BaseConector";
    
    Callbacks callbacks;

    ros::Subscriber joints = n.subscribe("/schunk/position/joint_states", 10,
		    &Callbacks::jointsCallback, &callbacks);


    ROS_DEBUG("Waiting for some joint state info\n");
    while (callbacks.ready == false) {
	    ros::spinOnce();
	    r.sleep();
    }

    ros::ServiceClient ik_service = n.serviceClient<
		    schunk_kinematics::GetVelocityIK> ("/schunk_kinematics/get_ik_vel");
    
    ros::ServiceClient fk_service = n.serviceClient<
		    kinematics_msgs::GetPositionFK> ("/schunk_kinematics/get_fk");

    ROS_DEBUG("Waiting for the kineamtics service...\n");
    ik_service.waitForExistence(ros::Duration(-1));
    fk_service.waitForExistence(ros::Duration(-1));
    

    schunk_kinematics::GetVelocityIK ik_msg;
    ik_msg.request.timeout = ros::Duration(10);
    ik_msg.request.ik_request.ik_link_name = "GripperBox";


    ik_msg.request.ik_request.twist.angular.z = 0;
    ik_msg.request.ik_request.twist.angular.y = 0;
    ik_msg.request.ik_request.twist.angular.x = 0;
    
    float velocity[3];
    velocity[0]=velocity[1]=velocity[2]=0;
    
    while (ros::ok()) {
   
	std::string err;
	//Getting the target point in the arm frame of reference	
	target_pose.header.stamp = ros::Time::now();
	
	bool waiting_result = transformer.waitForTransform("/schunk/position/GripperBox", "/head", 
					    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;
	gripper_roll = result_vector.x();
	gripper_pitch = result_vector.y();
	gripper_yaw = result_vector.z();
	
	double scale = 1;
	
	ik_msg.request.ik_request.twist.linear.x = 0;
	ik_msg.request.ik_request.twist.linear.y = 0;
	ik_msg.request.ik_request.twist.linear.z = 0;
	
	ik_msg.request.ik_request.twist.angular.x = gripper_roll * scale;
	ik_msg.request.ik_request.twist.angular.y = gripper_pitch * scale;
	ik_msg.request.ik_request.twist.angular.z = gripper_yaw * scale;
	
	ROS_INFO("Moving at vels (roll,pitch,yaw): %.2f, %.2f, %.2f", gripper_roll, 
			gripper_pitch, gripper_yaw);

	ik_msg.request.ik_request.robot_state.joint_state = callbacks.jointstates;


	if (ik_service.call(ik_msg)) {
		//		ROS_INFO("Sum: %ld", (long int)ik_msg.response.sum);
	} else {
	    while (! ik_service.waitForExistence(ros::Duration(5))) {
		ROS_DEBUG("Waiting for IK service");
	    }
	}

	vel_pub.publish(ik_msg.response.solution.joint_state);
// 	ROS_DEBUG("");


	r.sleep();
	ros::spinOnce();
    }

}
Beispiel #14
0
/*!
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);
}
Beispiel #15
0
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);
        backgroundBruite.display();
        
        regroupe.setTexture(backgroundBruite.getTexture());
        }
        
        sf::Event event;
        while (window.pollEvent(event))
        {
            // Close window: exit
            if (event.type == sf::Event::Closed) {
                window.close();
            }

            // Escape pressed: exit
            if (event.type == sf::Event::KeyPressed && event.key.code == sf::Keyboard::Escape) {
                window.close();
            }
            
            if (event.type == sf::Event::KeyPressed && event.key.code == sf::Keyboard::A) {
                if(stop == false)
                    stop = true;
                else
                    stop = false;
            }
        }

        // Clear screen
        window.clear();
        //window.draw(spriteTexteBruite);
        window.draw(regroupe);
        // Update the window
        window.display();
        
        if(stop){
        backgroundBruite.clear(sf::Color::White);
        backgroundBruite.draw(texteEcrit);
        backgroundBruite.display();
        }
    }

    return EXIT_SUCCESS;
}
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());

}
Beispiel #18
0
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);
        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)) {
            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;

        // 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.f;
        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.f, 1.f, 0.f)) * _orbRotation * rotation;
        
        //  Compute current velocity from position change
        glm::vec3 rawVelocity;
        if (deltaTime > 0.f) {
            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));

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

        // use the velocity to determine whether there's any movement (if the hand isn't new)
        const float MOVEMENT_DISTANCE_THRESHOLD = 0.003f;
        _amountMoved += rawVelocity * deltaTime;
        if (glm::length(_amountMoved) > MOVEMENT_DISTANCE_THRESHOLD && foundHand) {
            _lastMovement = usecTimestampNow();
            _amountMoved = glm::vec3(0.0f);
        }
        
        // 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.f) {
            palm->setTipVelocity((newTipPosition - oldTipPosition) / deltaTime);
        } else {
            palm->setTipVelocity(glm::vec3(0.f));
        }
        palm->setTipPosition(newTipPosition);
    }

    if (numActiveControllers == 2) {
        updateCalibration(controllers);
    }
#endif  // HAVE_SIXENSE
}
Beispiel #19
0
int main( int argc, char* argv [ ] )
{
   // In order to compile this, you need C ++ 11. If it doesn't compile, then
   // you can replace it by a sequence of push_backs.

   sequence seq({   rotation( cube::left ),
                    rotation( cube::left, 1, 3 ),
                    rotation( cube::up ),
                    rotation( cube::left, -1, 3 ),
                    rotation( cube::up, -1, 1 ),
                    rotation( cube::left, -1, 1 ),
                    rotation( cube::up, 1, 1 ),
                    rotation( cube::left, 1, 3 ),
                    rotation( cube::up, -1, 1 ),
                    rotation( cube::left, -1, 3 ) });

   std::cout << seq << "\n";
   std::cout << repeat( seq, 5 ) << "\n";

   sequence corners = repeat( sequence( { { cube::right },
                                          { cube::down, -1 },
                                          { cube::right, -1 },
                                          { cube::down } } ), 2 ) *
                         sequence( { rotation( cube::up ) } ) *
                      repeat( sequence( { { cube::down, -1 },
                                          { cube::right },
                                          { cube::down },
                                          { cube::right, -1 } } ), 2 ) *
                        sequence( { rotation( cube::up, -1 ) } );
   std::cout << corners << "\n";

   cube c(4);

   // Open an SFML window. Switch it on, if you want to use
   // graphics.

   // sf::Window
   //       wind( sf::VideoMode( 1024, 500, 32 ), "Rubik Cube" );

   char q = ' ';
   while( q != 'q' )
   {
      std::cout << c << "\n";
      // plot(c);
      // wind. Display( );
      // Plotting is nicer of course, but you need graphics.

      c. rotate( corners );
      q = getchar( );
   }

   return 0;
}
Beispiel #20
0
int main(int argc, char **argv) {
    char *input_img1 = NULL, *input_img2 = NULL;
    double deg = 0.0, scale_fac = 0.0, sigma = 0.0;
    int num_threads = 1, chunk_size = 1;
    
    // check input parameters
    if (argc <= 6){
        if (argc < 2){
            printUsage();
            exit(1);
        }            
        // parse the input parameters
        if (!strcmp(argv[1], "-m") || !strcmp(argv[1], "-mp")){
            printf("Motion Estimation in process ...\n");
            if (argc < 4 || argc > 6){
                printUsage();
                exit(1);
            }
            // assign parameters
            input_img1 = argv[2];
            input_img2 = argv[3];
            if(strcmp(argv[1], "-m") == 0)
			{
				motion_estimation(input_img1, input_img2);
			}
			else
			{
				num_threads = 8;
                if (argc == 5){
                    num_threads = atoi(argv[4]);
                }
				motion_estimation_parallel(input_img1, input_img2, num_threads);
			}
            
        }
        else if (!strcmp(argv[1], "-c") || !strcmp(argv[1], "-cp")){
            printf("Corner Detection in process ...\n");
            if (argc < 3 || argc > 5){
                printUsage();
                exit(1);
            }
            // assign parameters
            input_img1 = argv[2];
            
			 if(strcmp(argv[1], "-c") == 0)
			{
				cornerDetectionSequential(input_img1);
			}
			else
			{
				num_threads = 8;
                if (argc == 4){
                    num_threads = atoi(argv[3]);
                }
				cornerDetectionParallel(input_img1, num_threads);
			}

            
        }
        else if (!strcmp(argv[1], "-r") || !strcmp(argv[1], "-rp")){
            printf("Rotation in process ...\n");
            if (argc < 4 || argc > 6){
                printUsage();
                exit(1);
            }
            // assign parameters
            input_img1 = argv[2];
            deg = atof(argv[3]);
            
            if (!strcmp(argv[1], "-r")){
                rotation(input_img1, deg);
            }
            else {
                // assigne the number of threads and chunk size
                if (argc == 5){
                    num_threads = atoi(argv[4]);
                }
                else {
                    num_threads = atoi(argv[4]);
                    chunk_size = atoi(argv[5]);
                }
                rotation_parallel(input_img1, deg, num_threads, chunk_size);
            }
        }
        else if (!strcmp(argv[1], "-s") || !strcmp(argv[1], "-sp")){
            printf("Scaling in process ...\n");
            if (argc < 4 || argc > 6){
                printUsage();
                exit(1);
            }
            
            // --------------------Done by Mengyi Zhu----------------------------------
            //---------------------Here is scaling operation---------------------------
            //--------------------it will store the scaled version into scaling.bmp----
            // assign parameters
            input_img1 = argv[2];
            scale_fac = atof(argv[3]);
            num_threads = 1;
            
            //---------------------implemention the operation--------------------------
            if (!strcmp(argv[1], "-s"))
            	image_scaling(scale_fac, input_img1,"scaling.bmp");
            else if(!strcmp(argv[1], "-sp"))
            {
            	if (argc == 5)
            		num_threads = atoi(argv[4]);	
            	image_scaling_parallel(scale_fac, input_img1,"scaling.bmp", num_threads);
            }
            //-------------------------------------------------------------------------
            //-------------------------------------------------------------------------
            
        }
        else if (!strcmp(argv[1], "-g") || !strcmp(argv[1], "-gp")){
            printf("Gasussian Blur in process ...\n");
            if (argc < 4 || argc > 6){
                printUsage();
                exit(1);
            }
            // assign parameters
            input_img1 = argv[2];
            sigma = atof(argv[3]);
            
            if (!strcmp(argv[1], "-g")){
                gaussian_blur(input_img1, sigma);
            }
            else {
                // assigne the number of threads and chunk size
                if (argc == 5){
                    num_threads = atoi(argv[4]);
                }
                else {
                    num_threads = atoi(argv[4]);
                    chunk_size = atoi(argv[5]);
                }
                gaussian_blur_parallel(input_img1, sigma, num_threads, chunk_size);
            } 
            
        }
        else if (!strcmp(argv[1], "-o") || !strcmp(argv[1], "-op")){
            printf("Outline Detection in process ...\n");
            if (argc < 3 || argc > 5){
                printUsage();
                exit(1);
            }
            // assign parameters
            input_img1 = argv[2];
            
            // TODO: Ali, add your code here
            
        }
        else {
            if (argc == 6){
                printf("All-in-one in process ...\n");
                
                // assign parameters
                input_img1 = argv[1];
                input_img2 = argv[2];
                deg = atof(argv[3]);
                scale_fac = atof(argv[4]);
                sigma = atof(argv[5]);
                
                // Ding, add your sequential code here
                motion_estimation(input_img1, input_img2);
                // Michael's sequential code here
				cornerDetectionSequential(input_img1);
                // Haokun, add your sequential code here
                rotation(input_img1, deg);
                // --------------------Done by Mengyi Zhu---------------------------------
                //---------------------Here is scaling operation--------------------------
                //--------------------it will store the scaled version into scaling.bmp---
                image_scaling(scale_fac, input_img1,"scaling.bmp");
                //------------------------------------------------------------------------
                //------------------------------------------------------------------------
                // Xin, add your sequential code here
                gaussian_blur(input_img1, sigma);
                // TODO: Ali, add your sequential code here
            }
            else if (argc >= 7 && argc <= 9 && !strcmp(argv[1], "-p")){
                // assign parameters
                input_img1 = argv[2];
                input_img2 = argv[3];
                deg = atof(argv[4]);
                scale_fac = atof(argv[5]);
                sigma = atof(argv[6]);
                
                if (argc == 8){
                    num_threads = atoi(argv[7]);
                }
                else if (argc == 9){
                    num_threads = atoi(argv[7]);
                    chunk_size = atoi(argv[8]);
                }
                
                // Ding, add your parallel code here
                motion_estimation_parallel(input_img1, input_img2, num_threads);
                // Michael, add your parallel code here
				cornerDetectionParallel(input_img1,num_threads);
                // Haokun, add your parallel code here
                rotation_parallel(input_img1, deg, num_threads, chunk_size);
                // --------------------Done by Mengyi Zhu---------------------------------
                //---------------------Here is scaling operation-------------------------
                //--------------------it will store the scaled version into scaling.bmp----
                image_scaling_parallel(scale_fac, input_img1,"scaling.bmp", num_threads);
                //----------------------------------------------------------------------
                //----------------------------------------------------------------------
                // Xin, add your parallel code here
                gaussian_blur_parallel(input_img1, sigma, num_threads, chunk_size);
                // TODO: Ali, add your parallel code here
            }
            else {
                printf("Wrong option passed in or not enough parameters. Please refer the following usage.\n");
                printUsage();
                exit(1);
            }
        }
    }
    else {
        printUsage();
        exit(1);
    }
    
    return 0;   
}
Beispiel #21
0
qreal QElipseItem::GetAngle() //获得该图形的旋转角度
{
    return rotation();
}
Beispiel #22
0
void Enemy::move_forward()
{
    QLineF ln(pos(),dest);
    if(ln.length() < 5){
     if(point_index<points.size()-1){
        point_index++;
        dest = points[point_index];
        rotateToPoint(dest);}
    }




    int STEP_SIZE = 5;
    double theta = rotation(); // degrees

    double dy = STEP_SIZE * qSin(qDegreesToRadians(theta));
    double dx = STEP_SIZE * qCos(qDegreesToRadians(theta));

     QList<QGraphicsItem *> colliding_items = collidingItems();

    for (int i = 0, n = colliding_items.size(); i < n; ++i){
         if (typeid(*(colliding_items[i])) == typeid(Bullet)){
            scene()->removeItem(colliding_items[i]);
            delete colliding_items[i];
            this->set_Health(game->getShootdmg());
            if(this->Health <= 0){
            scene()->removeItem(this);
            delete this;
            game->score->increase();
            game->hscore->increase();
            }
            return;
        }
        if (typeid(*(colliding_items[i])) == typeid(slowdart)){




            scene()->removeItem(colliding_items[i]);
            this->timer->start(60*game->getSlowtime());
            delete colliding_items[i];



            return;
        }
        if (typeid(*(colliding_items[i])) == typeid(Home)){


            if(game->health->getHealth()!=10){
            scene()->removeItem(this);
            delete this;
            game->health->decrease(10);}
            else{
                scene()->removeItem(this);
                delete this;
                game->health->decrease(10);
                delete (game);
                go = new gameover;
                go->show();


            }
            return;
        }

        if (typeid(*(colliding_items[i])) == typeid(triplearrow)){
            scene()->removeItem(colliding_items[i]);
            delete colliding_items[i];
            this->set_Health(game->getBluedmg());
            if(this->Health <= 0){
            scene()->removeItem(this);
            delete this;
            game->score->increase();
            game->hscore->increase();
            }




            return;
        }
    }
    setPos(x()+dx, y()+dy);
}
Beispiel #23
0
void SixenseManager::update(float deltaTime) {
#ifdef HAVE_SIXENSE
    if (sixenseGetNumActiveControllers() == 0) {
        return;
    }
    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);
            printf("Found new Sixense controller, ID %i\n", data->controller_index);
        }
        
        palm->setActive(true);
        
        //  Read controller buttons and joystick into the hand
        palm->setControllerButtons(data->buttons);
        palm->setTrigger(data->trigger);
        palm->setJoystick(data->joystick_x, data->joystick_y);

        glm::vec3 position(data->pos[0], data->pos[1], data->pos[2]);
        // 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.f;
        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.f, 1.f, 0.f)) * _orbRotation * rotation;
        const glm::vec3 PALM_VECTOR(0.0f, -1.0f, 0.0f);
        glm::vec3 newNormal = rotation * PALM_VECTOR;
        palm->setRawNormal(newNormal);
        palm->setRawRotation(rotation);
        
        //  Compute current velocity from position change
        glm::vec3 rawVelocity = (position - palm->getRawPosition()) / deltaTime / 1000.f;
        palm->setRawVelocity(rawVelocity);   //  meters/sec
        palm->setRawPosition(position);
        
        // use the velocity to determine whether there's any movement (if the hand isn't new)
        const float MOVEMENT_SPEED_THRESHOLD = 0.05f;
        if (glm::length(rawVelocity) > MOVEMENT_SPEED_THRESHOLD && foundHand) {
            _lastMovement = usecTimestampNow();
        }
        
        // initialize the "finger" based on the direction
        FingerData finger(palm, hand);
        finger.setActive(true);
        finger.setRawRootPosition(position);
        const float FINGER_LENGTH = 300.0f;   //  Millimeters
        const glm::vec3 FINGER_VECTOR(0.0f, 0.0f, FINGER_LENGTH);
        const glm::vec3 newTipPosition = position + rotation * FINGER_VECTOR;
        finger.setRawTipPosition(position + rotation * FINGER_VECTOR);
        
        // Store the one fingertip in the palm structure so we can track velocity
        glm::vec3 oldTipPosition = palm->getTipRawPosition();
        palm->setTipVelocity((newTipPosition - oldTipPosition) / deltaTime / 1000.f);
        palm->setTipPosition(newTipPosition);
        
        // three fingers indicates to the skeleton that we have enough data to determine direction
        palm->getFingers().clear();
        palm->getFingers().push_back(finger);
        palm->getFingers().push_back(finger);
        palm->getFingers().push_back(finger);
    }

    if (numActiveControllers == 2) {
        updateCalibration(controllers);
    }

    // if the controllers haven't been moved in a while, disable
    const unsigned int MOVEMENT_DISABLE_DURATION = 30 * 1000 * 1000;
    if (usecTimestampNow() - _lastMovement > MOVEMENT_DISABLE_DURATION) {
        for (std::vector<PalmData>::iterator it = hand->getPalms().begin(); it != hand->getPalms().end(); it++) {
            it->setActive(false);
        }
    }
#endif  // HAVE_SIXENSE
}
void LightNode::selectPlanes(Selector& selector, SelectionTest& test, const PlaneCallback& selectedPlaneCallback) {
	test.BeginMesh(localToWorld());
	// greebo: Make sure to use the local lightAABB() for the selection test, excluding the light center
	AABB localLightAABB(Vector3(0,0,0), _light.getDoom3Radius().m_radiusTransformed);
	m_dragPlanes.selectPlanes(localLightAABB, selector, test, selectedPlaneCallback, rotation());
}
Beispiel #25
0
void GLC_Cone::createMeshAndWire()
{
	Q_ASSERT(GLC_Mesh::isEmpty());
	Q_ASSERT(m_WireData.isEmpty());

	// Create cosinus and sinus array according to the discretion and radius
	const int vertexNumber= m_Discret + 1;
	// Normals values
	QVector<float> cosNormalArray(vertexNumber);
	QVector<float> sinNormalArray(vertexNumber);

	QVector<float> cosArray(vertexNumber);
	QVector<float> sinArray(vertexNumber);

	const double angle= (2.0 * glc::PI) / static_cast<double>(m_Discret);

	// Normal Z value
	GLC_Vector3d normalVector(1.0, 0.0, 0.0);
	GLC_Matrix4x4 rotation(glc::Y_AXIS, -atan(m_Radius / m_Length));
	normalVector= rotation * normalVector;
	const float normalZ= static_cast<float>(normalVector.z());
	const double factor= normalVector.x(); // Normailsation factor

	for (int i= 0; i < vertexNumber; ++i)
	{
		const double cosValue= cos(static_cast<double>(i) * angle);
		const double sinValue= sin(static_cast<double>(i) * angle);

		cosNormalArray[i]= static_cast<GLfloat>(factor * cosValue);
		sinNormalArray[i]= static_cast<GLfloat>(factor * sinValue);

		cosArray[i]= static_cast<GLfloat>(m_Radius * cosValue);
		sinArray[i]= static_cast<GLfloat>(m_Radius * sinValue);
	}


	// Mesh Data
	GLfloatVector verticeVector;
	GLfloatVector normalsVector;
	GLfloatVector texelVector;

	// Wire Data
	GLfloatVector bottomWireData(vertexNumber * 3);

	const int size= vertexNumber * 3;
	verticeVector.resize(3 * size);
	normalsVector.resize(3 * size);
	texelVector.resize(2 * size);

	for (int i= 0; i < vertexNumber; ++i)
	{
		// Bottom Mesh
		verticeVector[3 * i]= cosArray[i];
		verticeVector[3 * i + 1]= sinArray[i];
		verticeVector[3 * i + 2]= 0.0f;

		normalsVector[3 * i]= cosNormalArray[i];
		normalsVector[3 * i + 1]= sinNormalArray[i];
		normalsVector[3 * i + 2]= normalZ;

		texelVector[2 * i]= static_cast<float>(i) / static_cast<float>(m_Discret);
		texelVector[2 * i + 1]= 0.0f;

		// Bottom Wire
		bottomWireData[3 * i]= cosArray[i];
		bottomWireData[3 * i + 1]= sinArray[i];
		bottomWireData[3 * i + 2]= 0.0f;

		// Top
		verticeVector[3 * i + 3 * vertexNumber]= 0.0f;
		verticeVector[3 * i + 1 + 3 * vertexNumber]= 0.0f;
		verticeVector[3 * i + 2 + 3 * vertexNumber]= static_cast<float>(m_Length);

		normalsVector[3 * i + 3 * vertexNumber]= cosNormalArray[i];
		normalsVector[3 * i + 1 + 3 * vertexNumber]= sinNormalArray[i];
		normalsVector[3 * i + 2 + 3 * vertexNumber]= normalZ;

		texelVector[2 * i + 2 * vertexNumber]= texelVector[i];
		texelVector[2 * i + 1 + 2 * vertexNumber]= 1.0f;

		// Bottom Cap ends
		verticeVector[3 * i + 2 * 3 * vertexNumber]= cosArray[i];
		verticeVector[3 * i + 1 + 2 * 3 * vertexNumber]= sinArray[i];
		verticeVector[3 * i + 2 + 2 * 3 * vertexNumber]= 0.0f;

		normalsVector[3 * i + 2 * 3 * vertexNumber]= 0.0f;
		normalsVector[3 * i + 1 + 2 * 3 * vertexNumber]= 0.0f;
		normalsVector[3 * i + 2 + 2 * 3 * vertexNumber]= -1.0f;

		texelVector[2 * i + 2 * 2 * vertexNumber]= texelVector[i];
		texelVector[2 * i + 1 + 2 * 2 * vertexNumber]= 0.0f;

	}

	// Add bulk data in to the mesh
	GLC_Mesh::addVertice(verticeVector);
	GLC_Mesh::addNormals(normalsVector);
	GLC_Mesh::addTexels(texelVector);

	// Add polyline to wire data
	GLC_Geometry::addPolyline(bottomWireData);

	// Set the material to use
	GLC_Material* pCylinderMaterial;
	if (hasMaterial())
	{
		pCylinderMaterial= this->firstMaterial();
	}
	else
	{
		pCylinderMaterial= new GLC_Material();
	}

	IndexList circumferenceStrips;
	// Create the index
	for (int i= 0; i < vertexNumber; ++i)
	{
		circumferenceStrips.append(i + vertexNumber);
		circumferenceStrips.append(i);
	}
	addTrianglesStrip(pCylinderMaterial, circumferenceStrips);

	{
		IndexList bottomCap;
		IndexList topCap;
		int id1= 0;
		int id2= m_Discret - 1;
		const int size= m_Discret / 2 + (m_Discret % 2);
		for (int i= 0; i < size; ++i)
		{
			bottomCap.append(id1 + 2 * vertexNumber);
			bottomCap.append(id2 + 2 * vertexNumber);

			id1+= 1;
			id2-= 1;
		}
		addTrianglesStrip(pCylinderMaterial, bottomCap);
	}

	finish();
}
void LightNode::selectReversedPlanes(Selector& selector, const SelectedPlanes& selectedPlanes)
{
	AABB localLightAABB(Vector3(0,0,0), _light.getDoom3Radius().m_radiusTransformed);
	m_dragPlanes.selectReversedPlanes(localLightAABB, selector, selectedPlanes, rotation());
}
Beispiel #27
0
void ShapeToGeometryVisitor::apply(const osg::Cone& cone)
{
    gl.PushMatrix();

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

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

    // evaluate hints
    bool createBody = (_hints ? _hints->getCreateBody() : true);
    bool createBottom = (_hints ? _hints->getCreateBottom() : true);

    unsigned int numSegments = 40;
    unsigned int numRows = 10;
    float ratio = (_hints ? _hints->getDetailRatio() : 1.0f);
    if (ratio > 0.0f && ratio != 1.0f) {
        numRows = (unsigned int) (numRows * ratio);
        if (numRows < MIN_NUM_ROWS)
            numRows = MIN_NUM_ROWS;
        numSegments = (unsigned int) (numSegments * ratio);
        if (numSegments < MIN_NUM_SEGMENTS)
            numSegments = MIN_NUM_SEGMENTS;
    }

    float r = cone.getRadius();
    float h = cone.getHeight();

    float normalz = r/(sqrtf(r*r+h*h));
    float normalRatio = 1.0f/(sqrtf(1.0f+normalz*normalz));
    normalz *= normalRatio;

    float angleDelta = 2.0f*osg::PI/(float)numSegments;
    float texCoordHorzDelta = 1.0/(float)numSegments;
    float texCoordRowDelta = 1.0/(float)numRows;
    float hDelta = cone.getHeight()/(float)numRows;
    float rDelta = cone.getRadius()/(float)numRows;

    float topz=cone.getHeight()+cone.getBaseOffset();
    float topr=0.0f;
    float topv=1.0f;
    float basez=topz-hDelta;
    float baser=rDelta;
    float basev=topv-texCoordRowDelta;
    float angle;
    float texCoord;

    if (createBody) {
        for(unsigned int rowi=0; rowi<numRows;
            ++rowi,topz=basez, basez-=hDelta, topr=baser, baser+=rDelta, topv=basev, basev-=texCoordRowDelta) {
                // we can't use a fan for the cone top
                // since we need different normals at the top
                // for each face..
                gl.Begin(GL_QUAD_STRIP);

                angle = 0.0f;
                texCoord = 0.0f;
                for(unsigned int topi=0; topi<numSegments;
                    ++topi,angle+=angleDelta,texCoord+=texCoordHorzDelta) {

                    float c = cosf(angle);
                    float s = sinf(angle);

                    gl.Normal3f(c*normalRatio,s*normalRatio,normalz);

                    gl.TexCoord2f(texCoord,topv);
                    gl.Vertex3f(c*topr,s*topr,topz);

                    gl.TexCoord2f(texCoord,basev);
                    gl.Vertex3f(c*baser,s*baser,basez);
                }

                // do last point by hand to ensure no round off errors.
                gl.Normal3f(normalRatio,0.0f,normalz);

                gl.TexCoord2f(1.0f,topv);
                gl.Vertex3f(topr,0.0f,topz);

                gl.TexCoord2f(1.0f,basev);
                gl.Vertex3f(baser,0.0f,basez);

                gl.End();
        }
    }

    if (createBottom) {
        gl.Begin(GL_TRIANGLE_FAN);

        angle = osg::PI*2.0f;
        texCoord = 1.0f;
        basez = cone.getBaseOffset();

        gl.Normal3f(0.0f,0.0f,-1.0f);
        gl.TexCoord2f(0.5f,0.5f);
        gl.Vertex3f(0.0f,0.0f,basez);

        for(unsigned int bottomi=0;bottomi<numSegments;
            ++bottomi,angle-=angleDelta,texCoord-=texCoordHorzDelta) {

            float c = cosf(angle);
            float s = sinf(angle);

            gl.TexCoord2f(c*0.5f+0.5f,s*0.5f+0.5f);
            gl.Vertex3f(c*r,s*r,basez);
        }

        gl.TexCoord2f(1.0f,0.0f);
        gl.Vertex3f(r,0.0f,basez);

        gl.End();
    }

    gl.PopMatrix();
}
Beispiel #28
0
 void Mesh::rotation(double thetaX, double thetaY, double thetaZ)
 {
     rotation(0, thetaX);
     rotation(1, thetaY);
     rotation(2, thetaZ);
 }
Beispiel #29
0
void ShapeToGeometryVisitor::apply(const osg::HeightField& field)
{
    if (field.getNumColumns()==0 || field.getNumRows()==0) return;

    gl.PushMatrix();

    gl.Translated(field.getOrigin().x(),field.getOrigin().y(),field.getOrigin().z());


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

    float dx = field.getXInterval();
    float dy = field.getYInterval();

    float du = 1.0f/((float)field.getNumColumns()-1.0f);
    float dv = 1.0f/((float)field.getNumRows()-1.0f);

    float vBase = 0.0f;

    osg::Vec3 vertTop;
    osg::Vec3 normTop;

    osg::Vec3 vertBase;
    osg::Vec3 normBase;

    if (field.getSkirtHeight()!=0.0f)
    {
        gl.Begin(GL_QUAD_STRIP);

        float u = 0.0f;

        // draw bottom skirt
        unsigned int col;
        vertTop.y() = 0.0f;
        for(col=0;col<field.getNumColumns();++col,u+=du)
        {
            vertTop.x() = dx*(float)col;
            vertTop.z() = field.getHeight(col,0);
            normTop.set(field.getNormal(col,0));

            gl.TexCoord2f(u,0.0f);
            gl.Normal3fv(normTop.ptr());

            gl.Vertex3fv(vertTop.ptr());

            vertTop.z()-=field.getSkirtHeight();

            gl.Vertex3fv(vertTop.ptr());
        }

        gl.End();

        // draw top skirt
        gl.Begin(GL_QUAD_STRIP);

        unsigned int row = field.getNumRows()-1;

        u = 0.0f;
        vertTop.y() = dy*(float)(row);
        for(col=0;col<field.getNumColumns();++col,u+=du)
        {
            vertTop.x() = dx*(float)col;
            vertTop.z() = field.getHeight(col,row);
            normTop.set(field.getNormal(col,row));

            gl.TexCoord2f(u,1.0f);
            gl.Normal3fv(normTop.ptr());

            gl.Vertex3f(vertTop.x(),vertTop.y(),vertTop.z()-field.getSkirtHeight());

            //vertTop.z()-=field.getSkirtHeight();

            gl.Vertex3fv(vertTop.ptr());
        }

        gl.End();
    }



    // draw each row of HeightField
    for(unsigned int row=0;row<field.getNumRows()-1;++row,vBase+=dv)
    {

        float vTop = vBase+dv;
        float u = 0.0f;


        gl.Begin(GL_QUAD_STRIP);

        // draw skirt at beginning of this row if required.
        if (field.getSkirtHeight()!=0.0f)
        {
            vertTop.set(0.0f,dy*(float)(row+1),field.getHeight(0,row+1)-field.getSkirtHeight());
            normTop.set(field.getNormal(0,row+1));

            vertBase.set(0.0f,dy*(float)row,field.getHeight(0,row)-field.getSkirtHeight());
            normBase.set(field.getNormal(0,row));

            gl.TexCoord2f(u,vTop);
            gl.Normal3fv(normTop.ptr());
            gl.Vertex3fv(vertTop.ptr());

            gl.TexCoord2f(u,vBase);
            gl.Normal3fv(normBase.ptr());
            gl.Vertex3fv(vertBase.ptr());
        }

        // draw the actual row
        for(unsigned int col=0;col<field.getNumColumns();++col,u+=du)
        {
            vertTop.set(dx*(float)col,dy*(float)(row+1),field.getHeight(col,row+1));
            normTop.set(field.getNormal(col,row+1));

            vertBase.set(dx*(float)col,dy*(float)row,field.getHeight(col,row));
            normBase.set(field.getNormal(col,row));

            gl.TexCoord2f(u,vTop);
            gl.Normal3fv(normTop.ptr());
            gl.Vertex3fv(vertTop.ptr());

            gl.TexCoord2f(u,vBase);
            gl.Normal3fv(normBase.ptr());
            gl.Vertex3fv(vertBase.ptr());

        }

        // draw skirt at end of this row if required.
        if (field.getSkirtHeight()!=0.0f)
        {

            vertBase.z()-=field.getSkirtHeight();
            vertTop.z()-=field.getSkirtHeight();

            gl.TexCoord2f(u,vTop);
            gl.Normal3fv(normTop.ptr());
            gl.Vertex3fv(vertTop.ptr());

            gl.TexCoord2f(u,vBase);
            gl.Normal3fv(normBase.ptr());
            gl.Vertex3fv(vertBase.ptr());
        }

        gl.End();
    }


    gl.PopMatrix();

}
void GtkViewWidget::repaint()	{
	GtkPaintContext &gtkContext = (GtkPaintContext&)view()->context();
	Angle angle = rotation();
	bool isRotated = (angle == DEGREES90) || (angle == DEGREES270);
	int w = isRotated ? myArea->allocation.height : myArea->allocation.width;
	int h = isRotated ? myArea->allocation.width : myArea->allocation.height;
	gtkContext.updatePixmap(myArea, w, h);
	view()->paint();
	switch (angle) {
		default:
			cleanOriginalPixbuf();
			cleanRotatedPixbuf();
			gdk_draw_pixmap(myArea->window, myArea->style->white_gc, gtkContext.pixmap(),
											0, 0, 0, 0, myArea->allocation.width, myArea->allocation.height);
			break;
		case DEGREES180:
			cleanRotatedPixbuf();
			if ((myOriginalPixbuf != 0) &&
					((gdk_pixbuf_get_width(myOriginalPixbuf) != w) ||
					 (gdk_pixbuf_get_height(myOriginalPixbuf) != h))) {
				cleanOriginalPixbuf();
			}
			if (myOriginalPixbuf == 0) {
				myOriginalPixbuf = gdk_pixbuf_new(GDK_COLORSPACE_RGB, false, 8, w, h);
				myImage = gdk_image_new(GDK_IMAGE_FASTEST, gdk_drawable_get_visual(gtkContext.pixmap()), w, h);
			}
			gdk_drawable_copy_to_image(gtkContext.pixmap(), myImage, 0, 0, 0, 0, w, h);
			gdk_pixbuf_get_from_image(myOriginalPixbuf, myImage, gdk_drawable_get_colormap(gtkContext.pixmap()),
																0, 0, 0, 0, w, h);
			::rotate180(myOriginalPixbuf);
			gdk_draw_pixbuf(myArea->window, myArea->style->white_gc, myOriginalPixbuf,
											0, 0, 0, 0, w, h, GDK_RGB_DITHER_NONE, 0, 0);
			break;
		case DEGREES90:
		case DEGREES270:
			if ((myOriginalPixbuf != 0) &&
					((gdk_pixbuf_get_width(myOriginalPixbuf) != w) ||
					 (gdk_pixbuf_get_height(myOriginalPixbuf) != h))) {
				cleanOriginalPixbuf();
			}
			if ((myRotatedPixbuf != 0) &&
					((gdk_pixbuf_get_width(myRotatedPixbuf) != h) ||
					 (gdk_pixbuf_get_height(myRotatedPixbuf) != w))) {
				cleanRotatedPixbuf();
			}
			if (myOriginalPixbuf == 0) {
				myOriginalPixbuf = gdk_pixbuf_new(GDK_COLORSPACE_RGB, false, 8, w, h);
				myImage = gdk_image_new(GDK_IMAGE_FASTEST, gdk_drawable_get_visual(gtkContext.pixmap()), w, h);
			}
			if (myRotatedPixbuf == 0) {
				myRotatedPixbuf = gdk_pixbuf_new(GDK_COLORSPACE_RGB, false, 8, h, w);
			}
			gdk_drawable_copy_to_image(gtkContext.pixmap(), myImage, 0, 0, 0, 0, w, h);
			gdk_pixbuf_get_from_image(myOriginalPixbuf, myImage, gdk_drawable_get_colormap(gtkContext.pixmap()),
																0, 0, 0, 0, w, h);
			::rotate90(myRotatedPixbuf, myOriginalPixbuf, angle == DEGREES90);
			gdk_draw_pixbuf(myArea->window, myArea->style->white_gc, myRotatedPixbuf,
											0, 0, 0, 0, h, w, GDK_RGB_DITHER_NONE, 0, 0);
			break;
	}
}