void Bullet::move(int to_x, int to_y) { x += fixtof(fixcos(theta)) * speed; y += fixtof(fixsin(theta)) * speed; set_boundaries(); }
void DataProcessor::process() { vector<pcl::PointCloud<pcl::PointXYZRGB> > region_cloud; //vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr > region_ptr; vector<geometry_msgs::Point> object_extent = find_extents(*object_cloud_); marker_pub_.publish(set_boundaries("base_link","obj_extent",1, visualization_msgs::Marker::LINE_STRIP,visualization_msgs::Marker::ADD, object_extent,0.005,1.0f,0.0f,0.0f,1.0)); //Pick up every block duplo_v1::Manipulate_Duplo grasp_call; vector<sensor_msgs::PointCloud2> bricks = cluster(object_cloud_, 0); ROS_INFO("Need to pick up %zu bricks", bricks.size()); if (bricks.size() == 0) { nocluster_count_++; new_cloud_wanted_ = true; } else { nocluster_count_ = 0; pick_n_place(bricks[0]); new_cloud_wanted_ = true; } if (nocluster_count_ == 3) { ROS_INFO("Table cleared!"); ros::shutdown(); } if (new_cloud_wanted_) { ROS_INFO("DUPLO: Asking for new point cloud."); //ros::Duration(2).sleep(); duplo_v1::Get_New_PCD srv_newpcd; srv_newpcd.request.question = true; if (client_newpcd_.call(srv_newpcd)) ROS_INFO("DUPLO: Requesting for new point cloud."); else { ROS_ERROR("Failed to call service get new pcd."); return; } new_cloud_wanted_ = false; } }
void Fluids::advect(float dt, int b, GLuint& d, GLuint d0, GLuint u, GLuint v) { setFrameBuffer(frambuffer_); do_advect_->setFloat("dt", dt); do_advect_->apply(); setRenderTexture(d); glDrawBuffer(GL_COLOR_ATTACHMENT0); checkFramebufferStatus(); //assert(do_advect_->samplers_.count("density") && do_advect_->samplers_.count("velocity")); glEnable(GL_TEXTURE_2D); set_texture_for_sampler(do_advect_, "d0", 0, d0); set_texture_for_sampler(do_advect_, "u", 1, u); set_texture_for_sampler(do_advect_, "v", 2, v); draw_quad(1); set_boundaries(b, p_prev_, d); glActiveTexture(GL_TEXTURE0 + 2); glBindTexture(GL_TEXTURE_2D, 0); glActiveTexture(GL_TEXTURE0 + 1); glBindTexture(GL_TEXTURE_2D, 0); glActiveTexture(GL_TEXTURE0); glBindTexture(GL_TEXTURE_2D, 0); glDisable(GL_TEXTURE_2D); glUseProgram(0); setRenderTexture(0,0); }
/** Default constructor for Level_1 * @param parent QWidget parent object of Level_1 */ Level_1::Level_1(QWidget *parent) : QWidget(parent), ui(new Ui::Level_1) { //setup the ui elements from the ui file ui->setupUi(this); //Connect instructions/controls pushbutton to show_help() QObject::connect(ui->help_button, SIGNAL(clicked()), this, SLOT(show_help())); //create a new graphics scene to display scene = new QGraphicsScene(this); //set the graphics view to show this scene ui->level1_graphicsView->setScene(scene); //set framerate related parameters //numbers found through experimentation of what felt like the right play speed max_frames_per_second = 50; frames_per_second = 33; time_change_amount = 7; //set scene boundaries to those of the qgraphicsview boundary_margin = 5; int view_width = ui->level1_graphicsView->geometry().width()-boundary_margin; int view_height = ui->level1_graphicsView->geometry().height()-boundary_margin; scene->setSceneRect(0,0,view_width,view_height); //set our boolean values // first game hasn't yet been played first_game = true; // not yet started started=false; // set current level and current game to one current_level=1; current_game=1; // haven't just won a level won_level=false; // haven't yet beat entire game won_game=false; // time warp is bonus feature (in which ball moves faster when paddle moves right, slower when paddle moves left) time_warp=false; // create our paddle objects and add them to the scene paddle_top = new Paddle(170,25); paddle_bottom = new Paddle(170,440); scene->addItem(paddle_top); scene->addItem(paddle_bottom); // create our ball object and add it to the scene ball = new Ball(); scene->addItem(ball); // set losing lines (and draw them) set_losing_lines(); // create bricks set_bricks(); // set boundaries (and draw them) for these scene, thus collisions against the sides will be detected set_boundaries(); // create text item to display when beat entire game win_text = new QGraphicsTextItem; win_text->setPos(50,200); win_text->setPlainText("You won!!!!!!!"); win_text->setScale(5); }
void Fluids::project(float dt, GLuint& u, GLuint& v, GLuint& p, GLuint& div) { setFrameBuffer(frambuffer_); // stage 1 do_project_a_->apply(); setRenderTexture(div,0); setRenderTexture(p,1); setRenderTexture(velocity_accum_u0t_,2); setRenderTexture(velocity_accum_v0t_,3); glDrawBuffers(4, drawbuffers); checkFramebufferStatus(); glEnable(GL_TEXTURE_2D); set_texture_for_sampler(do_project_a_, "u", 0, u); set_texture_for_sampler(do_project_a_, "v", 1, v); draw_quad(1); setRenderTexture(0,0); setRenderTexture(0,1); setRenderTexture(0,2); setRenderTexture(0,3); glActiveTexture(GL_TEXTURE0 + 3); glBindTexture(GL_TEXTURE_2D, 0); glActiveTexture(GL_TEXTURE0 + 2); glBindTexture(GL_TEXTURE_2D, 0); glActiveTexture(GL_TEXTURE0 + 1); glBindTexture(GL_TEXTURE_2D, 0); glActiveTexture(GL_TEXTURE0); glBindTexture(GL_TEXTURE_2D, 0); set_boundaries(0, p_prev_, div); glActiveTexture(GL_TEXTURE0); glBindTexture(GL_TEXTURE_2D, 0); setRenderTexture(0,0); // stage 2 do_project_b_->apply(); for(int i=0; i< NUM_OP; ++i) { setRenderTexture(p_prev_,0); glDrawBuffers(1, drawbuffers); checkFramebufferStatus(); set_texture_for_sampler(do_project_b_, "div", 0, div); set_texture_for_sampler(do_project_b_, "p", 1, p); draw_quad(1); swap(p_prev_, p); // p has result set_boundaries(0, p_prev_, p); do_project_b_->apply(); } setRenderTexture(0,0); // stage 3 do_project_c_->apply(); setRenderTexture(u,0); setRenderTexture(v,1); glDrawBuffers(2, drawbuffers); checkFramebufferStatus(); set_texture_for_sampler(do_project_c_, "p", 0, p); set_texture_for_sampler(do_project_c_, "u", 1, velocity_accum_u0t_); // same as u set_texture_for_sampler(do_project_c_, "v", 2, velocity_accum_v0t_); // same as v // we need those to make A = A + B assignment draw_quad(1); setRenderTexture(0,0); setRenderTexture(0,1); set_boundaries(1, p_prev_, u); set_boundaries(2, p_prev_, v); setRenderTexture(0,0); glActiveTexture(GL_TEXTURE0 + 2); glBindTexture(GL_TEXTURE_2D, 0); glActiveTexture(GL_TEXTURE0 + 1); glBindTexture(GL_TEXTURE_2D, 0); glActiveTexture(GL_TEXTURE0); glBindTexture(GL_TEXTURE_2D, 0); glDisable(GL_TEXTURE_2D); glUseProgram(0); setRenderTexture(0,0); setRenderTexture(1,0); setRenderTexture(2,0); setRenderTexture(3,0); }
// final result in x void Fluids::diffuse(float dt, int b, GLuint& temp_x, GLuint& x, GLuint& x0, const bool method1/* = false*/) { setFrameBuffer(frambuffer_); glsl_program* prog = method1 ? do_diffuse_method1_ : do_diffuse_simple_; if(method1) { setRenderTexture(x); glDrawBuffers(1, drawbuffers); checkFramebufferStatus(); do_copy_->apply(); set_texture_for_sampler(do_copy_, "tex", 0, x0); draw_quad(1); } prog->setFloat("dt", dt); prog->apply(); glEnable(GL_TEXTURE_2D); for(int i=0; i< NUM_OP; ++i) { // 2 different ways: both are a bit incorrect though :-) if(method1) setRenderTexture(temp_x); else setRenderTexture(x); glDrawBuffers(1, drawbuffers); checkFramebufferStatus(); if(method1) { // use temp texture for processing //set_texture_for_sampler(prog, "x", 0, x); //set_texture_for_sampler(prog, "x0", 1, x0); set_texture_for_sampler(prog, "x0", 1, x); draw_quad(1); swap(temp_x, x); } else { // change do_diffuse shader, so that only reads from x0 are done even at places where we read from x set_texture_for_sampler(prog, "x0", 0, x0); draw_quad(1); //glBindTexture(GL_TEXTURE_2D, 0); set_boundaries(b, x0, x); prog->setFloat("dt", dt); prog->apply(); swap(x0, x); } } //if(method1 ) // swap(x0, x); setRenderTexture(0,0); glActiveTexture(GL_TEXTURE1); glBindTexture(GL_TEXTURE_2D, 0); glActiveTexture(GL_TEXTURE0); glBindTexture(GL_TEXTURE_2D, 0); glDisable(GL_TEXTURE_2D); glUseProgram(0); #undef METHOD1 }
void Stokes::FACOps::smooth_Tackley_2D (SAMRAI::solv::SAMRAIVectorReal<double>& solution, const SAMRAI::solv::SAMRAIVectorReal<double>& residual, int ln, int num_sweeps, double residual_tolerance) { const int p_id(solution.getComponentDescriptorIndex(0)), p_rhs_id(residual.getComponentDescriptorIndex(0)), v_id(solution.getComponentDescriptorIndex(1)), v_rhs_id(residual.getComponentDescriptorIndex(1)); #ifdef DEBUG_CHECK_ASSERTIONS if (solution.getPatchHierarchy() != d_hierarchy || residual.getPatchHierarchy() != d_hierarchy) { TBOX_ERROR(d_object_name << ": Vector hierarchy does not match\n" "internal hierarchy."); } #endif boost::shared_ptr<SAMRAI::hier::PatchLevel> level = d_hierarchy->getPatchLevel(ln); /* Only need to sync the rhs once. This sync is needed because calculating a new pressure update requires computing in the ghost region so that the update for the velocity inside the box will be correct. */ p_refine_patch_strategy.setTargetDataId(p_id); v_refine_patch_strategy.setTargetDataId(v_id); set_boundaries(p_id,v_id,level,true); xeqScheduleGhostFillNoCoarse(p_rhs_id,v_rhs_id,ln); if (ln > d_ln_min) { /* * Perform a one-time transfer of data from coarser level, * to fill ghost boundaries that will not change through * the smoothing loop. */ xeqScheduleGhostFill(p_id, v_id, ln); } double theta_momentum=0.7; double theta_continuity=1.0; /* * Smooth the number of sweeps specified or until * the convergence is satisfactory. */ double maxres; /* * Instead of checking residual convergence globally, we check the * converged flag. This avoids possible round-off errors affecting * different processes differently, leading to disagreement on * whether to continue smoothing. */ const SAMRAI::hier::Index ip(1,0), jp(0,1); bool converged = false; for (int sweep=0; sweep < num_sweeps*(1<<(d_ln_max-ln)) && !converged; ++sweep) { maxres=0; /* vx sweep */ xeqScheduleGhostFillNoCoarse(p_id,invalid_id,ln); for(int rb=0;rb<2;++rb) { xeqScheduleGhostFillNoCoarse(invalid_id,v_id,ln); for (SAMRAI::hier::PatchLevel::Iterator pi(level->begin()); pi!=level->end(); ++pi) { boost::shared_ptr<SAMRAI::hier::Patch> patch = *pi; boost::shared_ptr<SAMRAI::pdat::CellData<double> > p_ptr = boost::dynamic_pointer_cast<SAMRAI::pdat::CellData<double> > (patch->getPatchData(p_id)); SAMRAI::pdat::CellData<double> &p(*p_ptr); boost::shared_ptr<SAMRAI::pdat::SideData<double> > v_ptr = boost::dynamic_pointer_cast<SAMRAI::pdat::SideData<double> > (patch->getPatchData(v_id)); SAMRAI::pdat::SideData<double> &v(*v_ptr); boost::shared_ptr<SAMRAI::pdat::SideData<double> > v_rhs_ptr = boost::dynamic_pointer_cast<SAMRAI::pdat::SideData<double> > (patch->getPatchData(v_rhs_id)); SAMRAI::pdat::SideData<double> &v_rhs(*v_rhs_ptr); boost::shared_ptr<SAMRAI::pdat::CellData<double> > cell_visc_ptr = boost::dynamic_pointer_cast<SAMRAI::pdat::CellData<double> > (patch->getPatchData(cell_viscosity_id)); SAMRAI::pdat::CellData<double> &cell_viscosity(*cell_visc_ptr); boost::shared_ptr<SAMRAI::pdat::NodeData<double> > edge_visc_ptr = boost::dynamic_pointer_cast<SAMRAI::pdat::NodeData<double> > (patch->getPatchData(edge_viscosity_id)); SAMRAI::pdat::NodeData<double> &edge_viscosity(*edge_visc_ptr); SAMRAI::hier::Box pbox=patch->getBox(); boost::shared_ptr<SAMRAI::geom::CartesianPatchGeometry> geom = boost::dynamic_pointer_cast<SAMRAI::geom::CartesianPatchGeometry> (patch->getPatchGeometry()); double dx = geom->getDx()[0]; double dy = geom->getDx()[1]; for(int j=pbox.lower(1); j<=pbox.upper(1); ++j) { /* Do the red-black skip */ int i_min=pbox.lower(0) + (abs(pbox.lower(0) + j + rb))%2; for(int i=i_min; i<=pbox.upper(0)+1; i+=2) { SAMRAI::pdat::CellIndex center(SAMRAI::tbox::Dimension(2)); center[0]=i; center[1]=j; /* Update v */ smooth_V_2D(0,pbox,geom,center,ip,jp, p,v,v_rhs,maxres,dx,dy,cell_viscosity, edge_viscosity,theta_momentum); } } } set_boundaries(invalid_id,v_id,level,true); } /* vy sweep */ for(int rb=0;rb<2;++rb) { xeqScheduleGhostFillNoCoarse(invalid_id,v_id,ln); for (SAMRAI::hier::PatchLevel::Iterator pi(level->begin()); pi!=level->end(); ++pi) { boost::shared_ptr<SAMRAI::hier::Patch> patch = *pi; boost::shared_ptr<SAMRAI::pdat::CellData<double> > p_ptr = boost::dynamic_pointer_cast<SAMRAI::pdat::CellData<double> > (patch->getPatchData(p_id)); SAMRAI::pdat::CellData<double> &p(*p_ptr); boost::shared_ptr<SAMRAI::pdat::SideData<double> > v_ptr = boost::dynamic_pointer_cast<SAMRAI::pdat::SideData<double> > (patch->getPatchData(v_id)); SAMRAI::pdat::SideData<double> &v(*v_ptr); boost::shared_ptr<SAMRAI::pdat::SideData<double> > v_rhs_ptr = boost::dynamic_pointer_cast<SAMRAI::pdat::SideData<double> > (patch->getPatchData(v_rhs_id)); SAMRAI::pdat::SideData<double> &v_rhs(*v_rhs_ptr); boost::shared_ptr<SAMRAI::pdat::CellData<double> > cell_visc_ptr = boost::dynamic_pointer_cast<SAMRAI::pdat::CellData<double> > (patch->getPatchData(cell_viscosity_id)); SAMRAI::pdat::CellData<double> &cell_viscosity(*cell_visc_ptr); boost::shared_ptr<SAMRAI::pdat::NodeData<double> > edge_visc_ptr = boost::dynamic_pointer_cast<SAMRAI::pdat::NodeData<double> > (patch->getPatchData(edge_viscosity_id)); SAMRAI::pdat::NodeData<double> &edge_viscosity(*edge_visc_ptr); SAMRAI::hier::Box pbox=patch->getBox(); boost::shared_ptr<SAMRAI::geom::CartesianPatchGeometry> geom = boost::dynamic_pointer_cast<SAMRAI::geom::CartesianPatchGeometry> (patch->getPatchGeometry()); double dx = geom->getDx()[0]; double dy = geom->getDx()[1]; for(int j=pbox.lower(1); j<=pbox.upper(1)+1; ++j) { /* Do the red-black skip */ int i_min=pbox.lower(0) + (abs(pbox.lower(0) + j + rb))%2; for(int i=i_min; i<=pbox.upper(0); i+=2) { SAMRAI::pdat::CellIndex center(SAMRAI::tbox::Dimension(2)); center[0]=i; center[1]=j; /* Update v */ smooth_V_2D(1,pbox,geom,center,jp,ip, p,v,v_rhs,maxres,dy,dx,cell_viscosity, edge_viscosity,theta_momentum); } } } set_boundaries(invalid_id,v_id,level,true); } /* p sweep No need for red-black, because dp does not depend on the pressure. */ xeqScheduleGhostFillNoCoarse(invalid_id,v_id,ln); for (SAMRAI::hier::PatchLevel::Iterator pi(level->begin()); pi!=level->end(); ++pi) { boost::shared_ptr<SAMRAI::hier::Patch> patch = *pi; boost::shared_ptr<SAMRAI::pdat::CellData<double> > p_ptr = boost::dynamic_pointer_cast<SAMRAI::pdat::CellData<double> > (patch->getPatchData(p_id)); SAMRAI::pdat::CellData<double> &p(*p_ptr); boost::shared_ptr<SAMRAI::pdat::CellData<double> > dp_ptr = boost::dynamic_pointer_cast<SAMRAI::pdat::CellData<double> > (patch->getPatchData(dp_id)); SAMRAI::pdat::CellData<double> &dp(*dp_ptr); boost::shared_ptr<SAMRAI::pdat::CellData<double> > p_rhs_ptr = boost::dynamic_pointer_cast<SAMRAI::pdat::CellData<double> > (patch->getPatchData(p_rhs_id)); SAMRAI::pdat::CellData<double> &p_rhs(*p_rhs_ptr); boost::shared_ptr<SAMRAI::pdat::SideData<double> > v_ptr = boost::dynamic_pointer_cast<SAMRAI::pdat::SideData<double> > (patch->getPatchData(v_id)); SAMRAI::pdat::SideData<double> &v(*v_ptr); boost::shared_ptr<SAMRAI::pdat::CellData<double> > cell_visc_ptr = boost::dynamic_pointer_cast<SAMRAI::pdat::CellData<double> > (patch->getPatchData(cell_viscosity_id)); SAMRAI::pdat::CellData<double> &cell_viscosity(*cell_visc_ptr); boost::shared_ptr<SAMRAI::pdat::NodeData<double> > edge_visc_ptr = boost::dynamic_pointer_cast<SAMRAI::pdat::NodeData<double> > (patch->getPatchData(edge_viscosity_id)); SAMRAI::pdat::NodeData<double> &edge_viscosity(*edge_visc_ptr); SAMRAI::hier::Box pbox=patch->getBox(); boost::shared_ptr<SAMRAI::geom::CartesianPatchGeometry> geom = boost::dynamic_pointer_cast<SAMRAI::geom::CartesianPatchGeometry> (patch->getPatchGeometry()); double dx = geom->getDx()[0]; double dy = geom->getDx()[1]; SAMRAI::pdat::CellIterator cend(SAMRAI::pdat::CellGeometry::end(pbox)); for(SAMRAI::pdat::CellIterator ci(SAMRAI::pdat::CellGeometry::begin(pbox)); ci!=cend; ++ci) { const SAMRAI::pdat::CellIndex ¢er(*ci); const SAMRAI::pdat::SideIndex x(center,0,SAMRAI::pdat::SideIndex::Lower), y(center,1,SAMRAI::pdat::SideIndex::Lower); /* Update p */ double dvx_dx=(v(x+ip) - v(x))/dx; double dvy_dy=(v(y+jp) - v(y))/dy; double delta_R_continuity= p_rhs(center) - dvx_dx - dvy_dy; /* No scaling here, though there should be. */ maxres=std::max(maxres,std::fabs(delta_R_continuity)); dp(center)=delta_R_continuity*theta_continuity /Stokes_dRc_dp_2D(pbox,center,x,y,cell_viscosity,edge_viscosity,v,dx,dy); p(center)+=dp(center); } } set_boundaries(p_id,invalid_id,level,true); /* fix v sweep */ xeqScheduleGhostFillNoCoarse(dp_id,invalid_id,ln); for (SAMRAI::hier::PatchLevel::Iterator pi(level->begin()); pi!=level->end(); ++pi) { boost::shared_ptr<SAMRAI::hier::Patch> patch = *pi; boost::shared_ptr<SAMRAI::pdat::CellData<double> > dp_ptr = boost::dynamic_pointer_cast<SAMRAI::pdat::CellData<double> > (patch->getPatchData(dp_id)); SAMRAI::pdat::CellData<double> &dp(*dp_ptr); boost::shared_ptr<SAMRAI::pdat::SideData<double> > v_ptr = boost::dynamic_pointer_cast<SAMRAI::pdat::SideData<double> > (patch->getPatchData(v_id)); SAMRAI::pdat::SideData<double> &v(*v_ptr); boost::shared_ptr<SAMRAI::pdat::CellData<double> > cell_visc_ptr = boost::dynamic_pointer_cast<SAMRAI::pdat::CellData<double> > (patch->getPatchData(cell_viscosity_id)); SAMRAI::pdat::CellData<double> &cell_viscosity(*cell_visc_ptr); boost::shared_ptr<SAMRAI::pdat::NodeData<double> > edge_visc_ptr = boost::dynamic_pointer_cast<SAMRAI::pdat::NodeData<double> > (patch->getPatchData(edge_viscosity_id)); SAMRAI::pdat::NodeData<double> &edge_viscosity(*edge_visc_ptr); SAMRAI::hier::Box pbox=patch->getBox(); boost::shared_ptr<SAMRAI::geom::CartesianPatchGeometry> geom = boost::dynamic_pointer_cast<SAMRAI::geom::CartesianPatchGeometry> (patch->getPatchGeometry()); double dx = geom->getDx()[0]; double dy = geom->getDx()[1]; pbox.growUpper(SAMRAI::hier::IntVector::getOne(d_dim)); SAMRAI::pdat::CellIterator cend(SAMRAI::pdat::CellGeometry::end(pbox)); for(SAMRAI::pdat::CellIterator ci(SAMRAI::pdat::CellGeometry::begin(pbox)); ci!=cend; ++ci) { const SAMRAI::pdat::CellIndex ¢er(*ci); const SAMRAI::pdat::SideIndex x(center,0,SAMRAI::pdat::SideIndex::Lower), y(center,1,SAMRAI::pdat::SideIndex::Lower); const SAMRAI::pdat::NodeIndex edge(center,SAMRAI::pdat::NodeIndex::LowerLeft); /* Update v */ if(center[1]<pbox.upper(1)) { if(!((center[0]==pbox.lower(0) && v(x-ip)==boundary_value) || (center[0]==pbox.upper(0) && v(x+ip)==boundary_value))) v(x)+=(dp(center) - dp(center-ip)) /(dx*Stokes_dRm_dv_2D(cell_viscosity,edge_viscosity,center, center-ip,edge+jp,edge,dx,dy)); } if(center[0]<pbox.upper(0)) { if(!((center[1]==pbox.lower(1) && v(y-jp)==boundary_value) || (center[1]==pbox.upper(1) && v(y+jp)==boundary_value))) v(y)+=(dp(center) - dp(center-jp)) /(dy*Stokes_dRm_dv_2D(cell_viscosity,edge_viscosity,center, center-jp,edge+ip,edge,dy,dx)); } } } set_boundaries(invalid_id,v_id,level,true); // if (residual_tolerance >= 0.0) { /* * Check for early end of sweeps due to convergence * only if it is numerically possible (user gave a * non negative value for residual tolerance). */ converged = maxres < residual_tolerance; const SAMRAI::tbox::SAMRAI_MPI& mpi(d_hierarchy->getMPI()); int tmp= converged ? 1 : 0; if (mpi.getSize() > 1) { mpi.AllReduce(&tmp, 1, MPI_MIN); } converged=(tmp==1); // if (d_enable_logging) // SAMRAI::tbox::plog // // << d_object_name << "\n" // << "Tackley " << ln << " " << sweep << " : " << maxres << "\n"; // } } }