void draw() { ++_frames; gpucast::gl::timer_guard full_frame("Frame total"); float near_clip = 0.01f * _bbox.size().abs(); float far_clip = 2.0f * _bbox.size().abs(); gpucast::math::matrix4f view = gpucast::math::lookat(0.0f, 0.0f, float(_bbox.size().abs()), 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f); gpucast::math::vec3f translation = _bbox.center(); gpucast::math::matrix4f model = gpucast::math::make_translation(shiftx(), shifty(), distance()) * rotation() * gpucast::math::make_translation(-translation[0], -translation[1], -translation[2]); gpucast::math::matrix4f proj = gpucast::math::perspective(50.0f, float(_width) / _height, near_clip, far_clip); auto mvp = proj * view * model; auto mvpi = gpucast::math::inverse(mvp); _renderer->set_nearfar(near_clip, far_clip); _renderer->set_resolution(_width, _height); _renderer->view_setup(view, model, proj); { //gpucast::gl::timer_guard t("renderer->begin_draw()"); _renderer->begin_draw(); } for (auto const& o : _objects) { //gpucast::gl::timer_guard t("Draw object"); if (_renderer->inside_frustum(*o)) { o->draw(*_renderer); } } { //gpucast::gl::timer_guard t("renderer->end_draw()"); _renderer->end_draw(); } { gpucast::gl::timer_guard t("fxaa blit"); glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); glClearColor(0.4, 0.0, 0.0, 1.0f); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT); _fxaa_program->begin(); _fxaa_program->set_uniform_matrix4fv("modelviewprojectioninverse", 1, false, &mvpi[0]); _fxaa_program->set_uniform_matrix4fv("modelviewprojection", 1, false, &mvp[0]); _fxaa_program->set_texture2d("colorbuffer", *_colorattachment, 1); _fxaa_program->set_texture2d("depthbuffer", *_depthattachment, 2); _sample_linear->bind(1); _sample_linear->bind(2); _fxaa_program->set_uniform1i("fxaa_mode", int(_fxaa)); _fxaa_program->set_uniform1i("width", GLsizei(_width)); _fxaa_program->set_uniform1i("height", GLsizei(_height)); _quad->draw(); _fxaa_program->end(); } }
void ShapeToGeometryVisitor::apply(const osg::Box& box) { // evaluate hints bool createBody = (_hints ? _hints->getCreateBody() : true); bool createTop = (_hints ? _hints->getCreateTop() : true); bool createBottom = (_hints ? _hints->getCreateBottom() : true); float dx = box.getHalfLengths().x(); float dy = box.getHalfLengths().y(); float dz = box.getHalfLengths().z(); gl.PushMatrix(); gl.Translated(box.getCenter().x(),box.getCenter().y(),box.getCenter().z()); if (!box.zeroRotation()) { osg::Matrixd rotation(box.computeRotationMatrix()); gl.MultMatrixd(rotation.ptr()); } gl.Begin(GL_QUADS); if (createBody) { // -ve y plane gl.Normal3f(0.0f,-1.0f,0.0f); gl.TexCoord2f(0.0f,1.0f); gl.Vertex3f(-dx,-dy,dz); gl.TexCoord2f(0.0f,0.0f); gl.Vertex3f(-dx,-dy,-dz); gl.TexCoord2f(1.0f,0.0f); gl.Vertex3f(dx,-dy,-dz); gl.TexCoord2f(1.0f,1.0f); gl.Vertex3f(dx,-dy,dz); // +ve y plane gl.Normal3f(0.0f,1.0f,0.0f); gl.TexCoord2f(0.0f,1.0f); gl.Vertex3f(dx,dy,dz); gl.TexCoord2f(0.0f,0.0f); gl.Vertex3f(dx,dy,-dz); gl.TexCoord2f(1.0f,0.0f); gl.Vertex3f(-dx,dy,-dz); gl.TexCoord2f(1.0f,1.0f); gl.Vertex3f(-dx,dy,dz); // +ve x plane gl.Normal3f(1.0f,0.0f,0.0f); gl.TexCoord2f(0.0f,1.0f); gl.Vertex3f(dx,-dy,dz); gl.TexCoord2f(0.0f,0.0f); gl.Vertex3f(dx,-dy,-dz); gl.TexCoord2f(1.0f,0.0f); gl.Vertex3f(dx,dy,-dz); gl.TexCoord2f(1.0f,1.0f); gl.Vertex3f(dx,dy,dz); // -ve x plane gl.Normal3f(-1.0f,0.0f,0.0f); gl.TexCoord2f(0.0f,1.0f); gl.Vertex3f(-dx,dy,dz); gl.TexCoord2f(0.0f,0.0f); gl.Vertex3f(-dx,dy,-dz); gl.TexCoord2f(1.0f,0.0f); gl.Vertex3f(-dx,-dy,-dz); gl.TexCoord2f(1.0f,1.0f); gl.Vertex3f(-dx,-dy,dz); } if (createTop) { // +ve z plane gl.Normal3f(0.0f,0.0f,1.0f); gl.TexCoord2f(0.0f,1.0f); gl.Vertex3f(-dx,dy,dz); gl.TexCoord2f(0.0f,0.0f); gl.Vertex3f(-dx,-dy,dz); gl.TexCoord2f(1.0f,0.0f); gl.Vertex3f(dx,-dy,dz); gl.TexCoord2f(1.0f,1.0f); gl.Vertex3f(dx,dy,dz); } 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(); }
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(); }
void TrackBall::stop() { m_rotation = rotation(); m_paused = true; }
RegularData3D* GridAnalysis::calculate() { // restore the ligand after all calculations, in case the scoring-function is also used for something else... AtomContainer* backup_ligand = scoring_function_->getLigand(); best_poses_.clear(); scoring_function_->setLigand(probe_group_); if (resolution_ < 0) // resolution has not been set manually { double radius = scoring_function_->getLigandRadius(); resolution_ = radius/2; if (resolution_ < 1) resolution_ = 1; } const HashGrid3<Atom*>* hashgrid = scoring_function_->getHashGrid(); origin_ = hashgrid->getOrigin(); Vector3 hashgrid_resolution = hashgrid->getUnit(); Size no_x_steps = (Size)((hashgrid->getSizeX()*hashgrid_resolution[0])/resolution_); Size no_y_steps = (Size)((hashgrid->getSizeY()*hashgrid_resolution[1])/resolution_); Size no_z_steps = (Size)((hashgrid->getSizeZ()*hashgrid_resolution[2])/resolution_); Vector3 dimension(no_x_steps*resolution_, no_y_steps*resolution_, no_z_steps*resolution_); Vector3 resolution(resolution_, resolution_, resolution_); RegularData3D* reg3d = new RegularData3D(origin_, dimension, resolution); Size no_atoms = scoring_function_->getNoLigandAtoms(); bool enable_rotation = 0; AtomIterator it = probe_group_.beginAtom(); if (it == probe_group_.endAtom()) { cerr<<"Error, probe group has no atoms!!"<<endl; return 0; } it++; if (it != probe_group_.endAtom()) enable_rotation = 1; center_ = scoring_function_->getLigandCenter(); for (Size x = 0; x < no_x_steps; x++) { for (Size y = 0; y < no_y_steps; y++) { for (Size z = 0; z < no_z_steps; z++) { Vector3 position(origin_[0]+(x+0.5)*resolution_, origin_[1]+(y+0.5)*resolution_, origin_[2]+(z+0.5)*resolution_); moveProbeGroup_(position); scoring_function_->update(); double score = scoring_function_->updateScore(); int best_angle_x = 0; int best_angle_y = 0; int best_angle_z = 0; // if probe-group has more than one atom, enable rotation around all three axes and store minimal score in RegularData3D if (enable_rotation && score < 0.75e10*no_atoms) { for (Size i = 1; i < 36; i++) { rotateProbeGroup_(0, 10); scoring_function_->update(); double score_i = scoring_function_->updateScore(); if (score_i < score) { score = score_i; best_angle_x = i*10; } } rotateProbeGroup_(0, -350+best_angle_x); for (Size i = 1; i < 36; i++) { rotateProbeGroup_(1, 10); scoring_function_->update(); double score_i = scoring_function_->updateScore(); if (score_i < score) { score = score_i; best_angle_y = i*10; } } rotateProbeGroup_(1, -350+best_angle_y); for (Size i = 1; i < 36; i++) { rotateProbeGroup_(2, 10); scoring_function_->update(); double score_i = scoring_function_->updateScore(); if (score_i < score) { score = score_i; best_angle_z = i*10; } } // rotate back to original orientation rotateProbeGroup_(2, -350); rotateProbeGroup_(1, -best_angle_y); rotateProbeGroup_(0, -best_angle_x); } 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); }
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())); }
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]); } } }
// 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; } } }
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 }
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(); } }
/*! sets the rotation of this node. The axis parameter will be transformed into 'relativeTo' space. So a passing in an axis of (0, 1, 0) with TS_Object will rotate the node around its own up axis. */ void SLNode::rotation(SLfloat angleDeg, const SLVec3f& axis, SLTransformSpace relativeTo) { SLQuat4f rot(angleDeg, axis); rotation(rot, relativeTo); }
int main(int, char const**) { // Create the main window sf::RenderWindow window(sf::VideoMode(800, 600), "SFML window"); /** * Bruit de base */ sf::Image imagebruitRandom; sf::Texture textureBruitRandom; sf::Sprite spriteBruitRandom; imagebruitRandom.create(800, 600); for(int i = 0; i < 800; i++) for(int j = 0; j < 600; j++) if(getIntRandom(0,1) == 1) imagebruitRandom.setPixel(i,j, sf::Color::White); textureBruitRandom.loadFromImage(imagebruitRandom); spriteBruitRandom.setTexture(textureBruitRandom); /*-----------------------*/ /** * Le background */ sf::Image backgroundMouvement = imagebruitRandom; sf::Texture textureBackgroundMouvement; sf::Sprite spriteBackgroundMouvement; /*------------------------*/ /** * Le texte */ sf::Text texteEcrit; sf::Font fontTexte; fontTexte.loadFromFile("/Users/nicolasserf/Desktop/ProjetPersonnel/monPremierBruit/monPremierBruit/sansation.ttf"); texteEcrit.setCharacterSize(100); texteEcrit.setColor(sf::Color::White); texteEcrit.setString("SALOPE"); texteEcrit.setFont(fontTexte); texteEcrit.setPosition(250, 300); /*-------------------------*/ /** * Texture texte "bruité" */ sf::RenderTexture texteBruite; texteBruite.create(800, 600); texteBruite.draw(texteEcrit); texteBruite.display(); texteBruite.draw(spriteBruitRandom, sf::BlendMultiply); texteBruite.display(); sf::Sprite spriteTexteBruite; spriteTexteBruite.setTexture(texteBruite.getTexture()); /*-------------------------*/ /** * Texture background "bruité" */ texteEcrit.setColor(sf::Color::Black); //on va écrire en noir sur fond blanc sf::RenderTexture backgroundBruite; backgroundBruite.create(800, 600); backgroundBruite.clear(sf::Color::White); //Fond blanc backgroundBruite.draw(texteEcrit); backgroundBruite.display(); /*------------------------*/ /** * Réunion du texte bruité et du background bruité */ sf::Sprite regroupe; bool stop = true; while (window.isOpen()) { if(stop) { backgroundMouvement = rotation(backgroundMouvement); textureBackgroundMouvement.loadFromImage(backgroundMouvement); spriteBackgroundMouvement.setTexture(textureBackgroundMouvement); backgroundBruite.draw(spriteBackgroundMouvement, sf::BlendMultiply); backgroundBruite.display(); regroupe.setTexture(backgroundBruite.getTexture()); backgroundBruite.draw(spriteTexteBruite, sf::BlendAdd); 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()); }
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 }
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; }
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; }
qreal QElipseItem::GetAngle() //获得该图形的旋转角度 { return rotation(); }
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); }
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()); }
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()); }
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(); }
void Mesh::rotation(double thetaX, double thetaY, double thetaZ) { rotation(0, thetaX); rotation(1, thetaY); rotation(2, thetaZ); }
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 >kContext = (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; } }