void MatrixLinearSystem<Scalar>::computeJacobiPreconditioner() {//jacobi preconditioner: diagonal matrix with entries of 1.0/A(i,i) if(coefficient_matrix_) { if(preconditioner_) delete preconditioner_; MatrixMxN<Scalar> *dense_A = coefficient_matrix_->denseMatrix(); if(dense_A) { MatrixMxN<Scalar> dense_T(dense_A->rows(),dense_A->cols(),0.0); PHYSIKA_ASSERT(dense_T.rows() == dense_T.cols()); for(unsigned int i = 0; i < dense_T.rows(); ++i) dense_T(i,i) = 1.0/(*dense_A)(i,i); preconditioner_ = new GeneralizedMatrix<Scalar>(dense_T); return; } SparseMatrix<Scalar> *sparse_A = coefficient_matrix_->sparseMatrix(); if(sparse_A) { SparseMatrix<Scalar> sparse_T(sparse_A->rows(),sparse_A->cols()); PHYSIKA_ASSERT(sparse_T.rows() == sparse_T.cols()); for(unsigned int i = 0; i < sparse_T.rows(); ++i) sparse_T.setEntry(i,i,1.0/(*sparse_A)(i,i)); preconditioner_ = new GeneralizedMatrix<Scalar>(sparse_T); return; } } else std::cerr<<"Warning: coefficient matrix not explicitly provided, computeJacobiPreconditioner() operation ignored!\n"; }
void MatrixMxN<Scalar>::allocMemory(unsigned int rows, unsigned int cols) { #ifdef PHYSIKA_USE_EIGEN_MATRIX ptr_eigen_matrix_MxN_ = new Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>(rows,cols); PHYSIKA_ASSERT(ptr_eigen_matrix_MxN_); #elif defined(PHYSIKA_USE_BUILT_IN_MATRIX) data_ = new Scalar[rows*cols]; PHYSIKA_ASSERT(data_); rows_ = rows; cols_ = cols; #endif }
void MPMSolidPluginRender<Scalar,Dim>::onEndTimeStep(Scalar time, Scalar dt) { //stop timer when a frame ends //stop simulation when maximum frame reached MPMSolid<Scalar,Dim> *driver = this->driver(); PHYSIKA_ASSERT(driver); unsigned int max_frame = driver->getEndFrame(); unsigned int start_frame = driver->getStartFrame(); Scalar frame_rate = driver->frameRate(); unsigned int cur_frame = static_cast<unsigned int>(time*frame_rate); unsigned int frame_last_step = static_cast<unsigned int>((time-dt)*frame_rate); if(cur_frame-frame_last_step==1) //ended a frame { std::cout<<"End Frame "<<cur_frame-1<<" "; if(driver->isTimerEnabled()) { timer_.stopTimer(); Scalar time_cur_frame = timer_.getElapsedTime(); total_time_ += time_cur_frame; std::cout<<time_cur_frame<<" s"; } std::cout<<"\n"; if(this->auto_capture_frame_) //write screen to file { std::string file_name("./screen_shots/frame_"); std::stringstream adaptor; adaptor<<cur_frame; std::string cur_frame_str; adaptor>>cur_frame_str; file_name += cur_frame_str+std::string(".png"); this->window_->saveScreen(file_name); }
void PDMPluginBoundary<Scalar,Dim>::applySphere() { PDMBase<Scalar,Dim> * driver = dynamic_cast<PDMBase<Scalar,Dim>*>(this->driver_); PHYSIKA_ASSERT(driver); driver->resetParticleForce(); Vector<Scalar, Dim> vel_unit = sphere_velocity_; unsigned int numParticles = driver->numSimParticles(); for (unsigned int par_idx = 0; par_idx<numParticles; par_idx++) { Vector<Scalar,Dim> position = driver->particleCurrentPosition(par_idx); Vector<Scalar,Dim> relative_pos = position-sphere_pos_; Scalar len = relative_pos.norm(); if (len < sphere_radius_) { relative_pos.normalize(); //driver->setParticleCurPos(par_idx, sphere_pos_ + sphere_radius_* relative_pos); Vector<Scalar, Dim> relative_vel = driver->particleVelocity(par_idx) - sphere_velocity_; Scalar relative_vel_norm = relative_vel.norm(); Scalar cos_theta = relative_vel.dot(relative_pos)/relative_vel_norm; if (cos_theta<0) { Vector<Scalar, Dim> vel_normal = (relative_vel_norm*cos_theta)*relative_pos; Vector<Scalar, Dim> vel = sphere_velocity_ - 2*vel_normal + relative_vel; driver->setParticleVelocity(par_idx, vel); } } } }
Vector<Scalar,Dim> PiecewiseCubicSpline<Scalar,Dim>::gradient(const Vector<Scalar,Dim> ¢er_to_x, Scalar R) const { PHYSIKA_ASSERT(R > 0); Scalar a = 1.0; Scalar h = 0.5*R; switch(Dim) { case 2: a = 15.0/(7*PI*h*h); break; case 3: a = 3.0/(2*PI*h*h*h); break; default: PHYSIKA_ERROR("Wrong dimension specified."); } Scalar r = center_to_x.norm(); Vector<Scalar,Dim> direction(0); if(r>std::numeric_limits<Scalar>::epsilon()) direction = center_to_x / r; else direction[0] = 1.0;//set direction to x axis when x is at center Scalar s = r/h; if(s>2) return 0*direction; else if(s>=1) return a*(2-s)*(2-s)*(-1)/(2.0*h)*direction; else if(s>=0) return a*(-2.0*s/h+3.0/2*s*s/h)*direction; else PHYSIKA_ERROR("r/R must be greater than zero."); }
Scalar PiecewiseCubicSpline<Scalar,Dim>::weight(const Vector<Scalar,Dim> ¢er_to_x, Scalar R) const { PHYSIKA_ASSERT(R > 0); Scalar a = 1.0; Scalar h = 0.5*R; switch(Dim) { case 2: a = 15.0/(7*PI*h*h); break; case 3: a = 3.0/(2*PI*h*h*h); break; default: PHYSIKA_ERROR("Wrong dimension specified."); } Scalar r = center_to_x.norm(); Scalar s = r/h; if(s>2) return 0; else if(s>=1) return a*(2.0-s)*(2.0-s)*(2.0-s)/6.0; else if(s>=0) return a*(2.0/3.0-s*s+1.0/2.0*s*s*s); else PHYSIKA_ERROR("r/R must be greater than zero."); }
void PDMPluginBoundary<Scalar,Dim>::onBeginTimeStep(Scalar time, Scalar dt) { // add additional force to specified particles PDMBase<Scalar,Dim> * driver = dynamic_cast<PDMBase<Scalar,Dim>*>(this->driver_); PHYSIKA_ASSERT(driver); if (this->enable_force_) { for (unsigned int i = 0; i < this->force_idx_vec_.size(); i++) { driver->addParticleForce(this->force_idx_vec_[i], this->force_vec_[i]); } } if (driver->isSimulationPause() == false && this->enable_bullet_) { bullet_pos_ += bullet_velocity_*dt; applyBullet(); } if (driver->isSimulationPause() == false && this->enable_sphere_) { sphere_pos_ += sphere_velocity_*dt; applySphere(); } }
Scalar DesbrunSpikyWeightFunction<Scalar,1>::laplacian(Scalar center_to_x, Scalar R) const { PHYSIKA_ASSERT(R > 0); Scalar a = 2.0/(pow(R,4)); Scalar r = abs(center_to_x); return (r>R) ? 0 : a*(6.0)*(R - r); }
void PDMPluginBoundary<Scalar,Dim>::applyBullet() { PDMBase<Scalar,Dim> * driver = dynamic_cast<PDMBase<Scalar,Dim>*>(this->driver_); PHYSIKA_ASSERT(driver); driver->resetParticleForce(); Vector<Scalar, Dim> vel_unit = bullet_velocity_; vel_unit.normalize(); unsigned int numParticles = driver->numSimParticles(); for (unsigned int par_idx = 0; par_idx<numParticles; par_idx++) { Vector<Scalar,Dim> position = driver->particleCurrentPosition(par_idx); Vector<Scalar,Dim> relative_pos = position-bullet_pos_; Scalar len = relative_pos.norm(); if(len >= 0.75*bullet_radius_ && len <= 1.5*bullet_radius_) { Vector<Scalar,Dim> project_unit = relative_pos.normalize(); Vector<Scalar,Dim> force_dir = 0.5*(vel_unit+project_unit); force_dir.normalize(); Vector<Scalar,Dim> force = Ks_*pow((16*(len-0.75*bullet_radius_)*pow(len-1.5*bullet_radius_,2)),2.0/3)*force_dir; driver->addParticleForce(par_idx,force); } } }
void PDMPluginBoundary<Scalar,Dim>::onEndTimeStep(Scalar time, Scalar dt) { PDMBase<Scalar,Dim> * driver = dynamic_cast<PDMBase<Scalar,Dim>*>(this->driver_); PHYSIKA_ASSERT(driver); if (this->enable_floor_) { unsigned int numParticles = driver->numSimParticles(); for (unsigned int par_idx = 0; par_idx<numParticles; par_idx++) { Vector<Scalar, Dim> pos = driver->particleCurrentPosition(par_idx); if(pos[1]<this->floor_pos_) { pos[1] = this->floor_pos_; // set particle position and velocity driver->setParticleDisplacement(par_idx, pos-driver->particleRestPosition(par_idx)); Vector<Scalar, Dim> vel = driver->particleVelocity(par_idx); vel[1] = -this->recovery_coefficient_*vel[1]; driver->setParticleVelocity(par_idx,vel); } } } if (this->enable_fix_) { for (unsigned int id =0; id<this->fixed_idx_vec_.size(); id++) { unsigned int par_idx = this->fixed_idx_vec_[id]; driver->setParticleDisplacement(par_idx,Vector<Scalar,Dim>(0.0)); driver->setParticleVelocity(par_idx,Vector<Scalar,Dim>(0.0)); } } }
Scalar DesbrunSpikyWeightFunction<Scalar,1>::weight(Scalar center_to_x, Scalar R) const { PHYSIKA_ASSERT(R > 0); Scalar a = 2.0/(pow(R,4)); Scalar r = abs(center_to_x); return a*pow(R-r, 3); }
Scalar PiecewiseCubicSpline<Scalar,Dim>::laplacian(const Vector<Scalar,Dim> ¢er_to_x, Scalar R) const { PHYSIKA_ASSERT(R > 0); Scalar a = 1.0; Scalar h = 0.5*R; switch(Dim) { case 2: a = 15.0/(7*PI*h*h); break; case 3: a = 3.0/(2*PI*h*h*h); break; default: PHYSIKA_ERROR("Wrong dimension specified."); } Scalar r = center_to_x.norm(); Scalar s = r/h; if(s>2) return 0; else if(s>=1) { Scalar result = a/(-2*h)*(Dim*(4.0/r-4.0/h+r/(h*h))-4.0/r+r/(h*h)); return result; } else if(s>=0) { Scalar result = a*(Dim*(-2.0/(h*h)+3.0/(2.0*h*h*h)*r)+3.0/(2.0*h*h*h)*r); return result; } else PHYSIKA_ERROR("r/R must be greater than zero."); }
Scalar DesbrunSpikyWeightFunction<Scalar,1>::gradient(Scalar center_to_x, Scalar R) const { PHYSIKA_ASSERT(R > 0); Scalar a = 2.0/(pow(R,4)); Scalar r = abs(center_to_x); Scalar sign = center_to_x>=0 ? 1 : -1; return (r>R) ? 0 : a*(-3.0)*pow((R - r),2)*sign; }
void VectorND<Scalar>::allocMemory(unsigned int dims) { #ifdef PHYSIKA_USE_BUILT_IN_VECTOR data_ = new Scalar[dims]; dims_ = dims; PHYSIKA_ASSERT(data_); #endif }
void PDMPluginBoundary<Scalar,Dim>::addFixedParticle(unsigned int par_idx) { PDMBase<Scalar,Dim> * driver = dynamic_cast<PDMBase<Scalar,Dim>*>(this->driver_); PHYSIKA_ASSERT(driver); if(par_idx >= driver->numSimParticles()) { std::cerr<<"particle idx out of range.\n"; std::exit(EXIT_FAILURE); } this->fixed_idx_vec_.push_back(par_idx); }
void PDMPluginBoundary<Scalar,Dim>::addParticleAdditionalForce(unsigned int par_idx, const Vector<Scalar, Dim> & f) { PDMBase<Scalar,Dim> * driver = dynamic_cast<PDMBase<Scalar,Dim>*>(this->driver_); PHYSIKA_ASSERT(driver); if(par_idx >= driver->numSimParticles()) { std::cerr<<"particle idx out of range.\n"; std::exit(EXIT_FAILURE); } this->force_idx_vec_.push_back(par_idx); this->force_vec_.push_back(f); }
void RigidDriverPluginRender<Scalar, Dim>::display() { GlutWindow *window = active_render_->window_; PHYSIKA_ASSERT(window); Color<double> background_color = window->backgroundColor<double>(); glClearColor(background_color.redChannel(), background_color.greenChannel(), background_color.blueChannel(), background_color.alphaChannel()); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); window->applyCameraAndLights(); if(active_render_->is_render_contact_face_ && active_render_->contact_face_ids_ != NULL)//render contact faces { SurfaceMeshRender<Scalar>* render; unsigned int num_body = active_render_->rigid_driver_->numRigidBody(); for(unsigned int i = 0; i < num_body; ++i) { render = dynamic_cast<SurfaceMeshRender<Scalar>*>(active_render_->render_queue_[i]); if(render == NULL) continue; //render->synchronize(); render->renderFaceWithColor((active_render_->contact_face_ids_)[i], Color<float>::Blue()); } } if(active_render_->is_render_contact_normal_)//render contact normals { glPushAttrib(GL_ALL_ATTRIB_BITS); glPushMatrix(); openGLColor3(Color<double>::Red()); glBegin(GL_LINES); unsigned int num_normal = static_cast<unsigned int>(active_render_->contact_normal_positions_.size()); for(unsigned int i = 0; i < num_normal; ++i) { openGLVertex(active_render_->contact_normal_positions_[i]); openGLVertex(active_render_->contact_normal_positions_[i] + active_render_->contact_normal_orientation_[i] * active_render_->normal_length_); } glEnd(); glPopMatrix(); glPopAttrib(); } (window->renderManager()).renderAll(); //render all tasks of render manager window->displayFrameRate(); glutSwapBuffers(); if(active_render_->rigid_driver_->step() % active_render_->screen_save_interval_ == 0) { std::stringstream adaptor; adaptor << active_render_->rigid_driver_->step() / active_render_->screen_save_interval_; std::string index_str; adaptor >> index_str; window->saveScreen(active_render_->screen_save_name_ + index_str + ".png"); }
LineGLCudaBuffer LineRenderUtil::mapLineGLCudaBuffer(unsigned int line_num) { if(cuda_vbo_mapper_ == nullptr || (line_num != 0 && line_num_ != line_num )) { cuda_vbo_mapper_ = std::make_shared<VBOCudaMapper>(line_VBO_, sizeof(float) * 6 * line_num); line_num_ = line_num; } const std::pair<float *, unsigned int> & res = cuda_vbo_mapper_->mapVBOBuffer(); PHYSIKA_ASSERT(line_num_ == res.second / sizeof(float) / 6); return LineGLCudaBuffer(res.first, line_num_); }
Scalar PiecewiseCubicSpline<Scalar,1>::laplacian(Scalar center_to_x, Scalar R) const { PHYSIKA_ASSERT(R > 0); Scalar h = 0.5*R; Scalar a = 1.0/h; Scalar r = abs(center_to_x); Scalar s = r/h; if(s>2) return 0; else if(s>=1) return a/(-2*h)*(-4/h+2*r/(h*h)); else if(s>=0) return a*(-2/(h*h)+3/(2*h*h*h)*r+3/(2*h*h*h)*r); else PHYSIKA_ERROR("r/R must be greater than zero."); }
void MPMSolidPluginRender<Scalar,Dim>::onBeginTimeStep(Scalar time, Scalar dt) { //start timer when the first frame begins MPMSolid<Scalar,Dim> *driver = this->driver(); PHYSIKA_ASSERT(driver); Scalar frame_rate = driver->frameRate(); Scalar cur_frame_scalar = time*frame_rate; unsigned int cur_frame = static_cast<unsigned int>(cur_frame_scalar); unsigned int start_frame = driver->getStartFrame(); if (cur_frame_scalar - start_frame < std::numeric_limits<Scalar>::epsilon()) //begins the first frame { if(driver->isTimerEnabled()) timer_.startTimer(); std::cout<<"Begin Frame "<<cur_frame<<"\n"; } }
Scalar PiecewiseCubicSpline<Scalar,1>::weight(Scalar center_to_x, Scalar R) const { PHYSIKA_ASSERT(R > 0); Scalar h = 0.5*R; Scalar a = 1.0/h; Scalar r = abs(center_to_x); Scalar s = r/h; if(s>2) return 0; else if(s>=1) return a*(2.0-s)*(2.0-s)*(2.0-s)/6.0; else if(s>=0) return a*(2.0/3.0-s*s+1.0/2.0*s*s*s); else PHYSIKA_ERROR("r/R must be greater than zero."); }
Scalar PiecewiseCubicSpline<Scalar,1>::gradient(Scalar center_to_x, Scalar R) const { PHYSIKA_ASSERT(R > 0); Scalar h = 0.5*R; Scalar a = 1.0/h; Scalar r = abs(center_to_x); Scalar sign = center_to_x>=0 ? 1 : -1; Scalar s = r/h; if(s>2) return 0; else if(s>=1) return a*(2-s)*(2-s)*(-1)/(2.0*h)*sign; else if(s>=0) return a*(-2.0*s/h+3.0/2*s*s/h)*sign; else PHYSIKA_ERROR("r/R must be greater than zero."); }
Scalar DesbrunSpikyWeightFunction<Scalar,Dim>::weight(const Vector<Scalar,Dim> ¢er_to_x, Scalar R) const { PHYSIKA_ASSERT(R > 0); Scalar a = 1.0; switch(Dim) { case 2: a = 10.0/(PI*pow(R,5)); break; case 3: a = 15.0/(PI*pow(R,6)); break; default: PHYSIKA_ERROR("Wrong dimension specified."); } Scalar r = center_to_x.norm(); return (r>R) ? 0 : a*pow((R - r),3); }
Scalar DesbrunSpikyWeightFunction<Scalar,Dim>::laplacian(const Vector<Scalar,Dim> ¢er_to_x, Scalar R) const { PHYSIKA_ASSERT(R > 0); Scalar a = 1.0; switch(Dim) { case 2: a = 10.0/(PI*pow(R,5)); break; case 3: a = 15.0/(PI*pow(R,6)); break; default: PHYSIKA_ERROR("Wrong dimension specified."); } Scalar r = center_to_x.norm(); if(r > R) return 0; Scalar result = 3.0*a*(r*r*(R*R/pow(r,3) - 1.0/r) - Dim*(R*R/pow(r,3) -2.0*R + r)); return result; }
Vector<Scalar,Dim> DesbrunSpikyWeightFunction<Scalar,Dim>::gradient(const Vector<Scalar,Dim> ¢er_to_x, Scalar R) const { PHYSIKA_ASSERT(R > 0); Scalar a = 1.0; switch(Dim) { case 2: a = 10.0/(PI*pow(R,5)); break; case 3: a = 15.0/(PI*pow(R,6)); break; default: PHYSIKA_ERROR("Wrong dimension specified."); } Scalar r = center_to_x.norm(); Vector<Scalar,Dim> direction(0); if(r>std::numeric_limits<Scalar>::epsilon()) direction = center_to_x/r; else direction[0] = 1.0;//set direction to x axis when x is at center return (r>R) ? 0*direction : a*(-3.0)*pow((R - r),2)*direction; }
void PDMImpactMethodBullet<Scalar,Dim>::applyImpact(Scalar dt) { PDMBase<Scalar,Dim> * driver = dynamic_cast<PDMBase<Scalar,Dim>*>(this->driver_); PHYSIKA_ASSERT(driver); // refresh impact pos this->impact_pos_ += dt*this->impact_velocity_; unsigned int numParticles = driver->numSimParticles(); unsigned int sphere_contact_num = 0; unsigned int cylinder_contact_num = 0; Scalar impact_velocity_norm = this->impact_velocity_.norm(); Vector<Scalar, Dim> unit_impact_velocity = this->impact_velocity_; unit_impact_velocity.normalize(); for (long long par_idx = 0; par_idx < numParticles; par_idx++) { Vector<Scalar, Dim> pos = driver->particleCurrentPosition(par_idx); //for right sphere Vector<Scalar, Dim> relative_pos = pos - impact_pos_; Vector<Scalar, Dim> unit_relative_pos = relative_pos; unit_relative_pos.normalize(); Scalar sphere_dist = relative_pos.norm(); bool is_inside_right_sphere = relative_pos.dot(unit_impact_velocity)>0 ? true:false; //for cylinder Scalar sin_theta = Vector<Scalar, Dim>(unit_relative_pos.cross(unit_impact_velocity)).norm(); // Vector<Scalar, Dim> is used to pass the complie if(Dim == 2) sin_theta /= 1.4142136; // special handling if Dim == 2 Scalar cylinder_dist = sin_theta * sphere_dist; Vector<Scalar, Dim> relative_to_end_cylinder_pos = relative_pos + bullet_cylinder_length_*unit_impact_velocity; bool is_inside_cylinder = (is_inside_right_sphere == false) && (relative_to_end_cylinder_pos.dot(unit_impact_velocity) > 0); if (sphere_dist < impact_radius_ && is_inside_right_sphere) { sphere_contact_num ++; // reset position if (this->enable_project_pos_ == true) { Vector<Scalar, Dim> new_pos = impact_pos_ + impact_radius_* unit_relative_pos; driver->setParticleCurPos(par_idx, new_pos); } // reset velocity Vector<Scalar, Dim> relative_vel = driver->particleVelocity(par_idx) - impact_velocity_; Scalar relative_vel_norm = relative_vel.norm(); Scalar cos_theta = relative_vel.dot(unit_relative_pos)/relative_vel_norm; if (cos_theta < 0) { Vector<Scalar, Dim> vel_normal = (relative_vel_norm*cos_theta)*unit_relative_pos; Vector<Scalar, Dim> new_vel = impact_velocity_ - (1+recovery_coefficient_)*vel_normal + relative_vel; driver->setParticleVelocity(par_idx, new_vel); } } else if (cylinder_dist < impact_radius_ && is_inside_cylinder) { cylinder_contact_num ++; Vector<Scalar, Dim> tangent_component_relative_pos = relative_pos.dot(unit_impact_velocity)*unit_impact_velocity; Vector<Scalar, Dim> normal_component_relative_pos = relative_pos - tangent_component_relative_pos; Vector<Scalar, Dim> unit_normal_component_relative_pos = normal_component_relative_pos; unit_normal_component_relative_pos.normalize(); // reset position if (this->enable_project_pos_ == true) { Vector<Scalar, Dim> new_pos = pos + (impact_radius_ - cylinder_dist)*unit_normal_component_relative_pos; driver->setParticleCurPos(par_idx, new_pos); } //reset velocity Vector<Scalar, Dim> vel = driver->particleVelocity(par_idx); Scalar vel_normal_norm = vel.dot(unit_normal_component_relative_pos); if (vel_normal_norm < 0) { Vector<Scalar, Dim> vel_normal = vel_normal_norm*unit_normal_component_relative_pos; Vector<Scalar, Dim> new_vel = vel - (1+recovery_coefficient_)*vel_normal; driver->setParticleVelocity(par_idx, new_vel); } } } std::cout<<"--------------------------------------------------------------------"<<std::endl; std::cout<<"impact pos: "<<this->impact_pos_<<std::endl; std::cout<<"particle inside sphere: "<<sphere_contact_num<<std::endl; std::cout<<"particle is inside cylinder: "<<cylinder_contact_num<<std::endl; std::cout<<"--------------------------------------------------------------------"<<std::endl; }