void PbfSolver::Update(float dt) { // reset the particle records ResetParticleRecords_(); RecordOldPositions_(); // apply the gravity and the boundary constraint gravity_.Evaluate(dt, ps_); ImposeBoundaryConstraint_(); // update the particle positions in the spatial hash // and find the neighbors for each particle. spatial_hash_.UpdateAll(); FindNeighbors_(); for (unsigned itr = 0; itr < num_iters_; ++itr) { // Compute lambda for (size_t p_i = 0; p_i < ps_->NumParticles(); ++p_i) { auto &ptc_rec_i = ptc_records_[p_i]; ptc_rec_i.lambda = ComputeLambda_(p_i); } // Compute delta position for (size_t p_i = 0; p_i < ps_->NumParticles(); ++p_i) { auto &ptc_rec_i = ptc_records_[p_i]; ptc_rec_i.delta_pos = ComputeDeltaPos_(p_i); } // Update particle position for (size_t p_i = 0; p_i < ps_->NumParticles(); ++p_i) { auto ptc_i = ps_->Get(p_i); auto new_pos_i = ptc_i.position() + ptc_records_[p_i].delta_pos; ptc_i.set_position(new_pos_i); } } ImposeBoundaryConstraint_(); // Update velocity for (size_t p_i = 0; p_i < ps_->NumParticles(); ++p_i) { auto ptc_i = ps_->Get(p_i); const auto old_pos_i = ptc_records_[p_i].old_pos; const auto new_pos_i = ptc_i.position(); const auto new_vel_i = (new_pos_i - old_pos_i) / dt; ptc_i.set_velocity(new_vel_i); } // Compute Vorticity for (size_t p_i = 0; p_i < ps_->NumParticles(); ++p_i) { auto &ptc_rec_i = ptc_records_[p_i]; ptc_rec_i.vorticity = ComputeVorticity_(p_i); } // Apply vorticity confinement and XSPH for (size_t p_i = 0; p_i < ps_->NumParticles(); ++p_i) { auto ptc_i = ps_->Get(p_i); const vec_t vel_i = ptc_i.velocity(); vec_t new_vel_i = vel_i; new_vel_i += (ComputeVorticityCorrForce_(p_i) * dt); new_vel_i += ComputeXsph_(p_i); ptc_i.set_velocity(new_vel_i); } }
void NormalPhysicsObject::update(int milliseconds) { double t = milliseconds / 1000.0; Vector3d acceleration = inverse_mass() * accumulated_force(); set_position(position() + velocity() * t + 0.5 * acceleration * t * t); set_velocity(velocity() + acceleration * t); // Damp the velocity where necessary. set_velocity(velocity() * m_dampingFactor); }
bool start_centering() { if(_sense.is_centered() == 0) { set_velocity(0); return true; } set_velocity(direction() * Cart::_slowest_velocity); _state = centering; return false; }
void ConvectionDiffusionBase<TDomain>::set_velocity(const std::vector<number>& vVel) { bool bZero = true; for(size_t i = 0; i < vVel.size(); ++i){ if(vVel[i] != 0.0) bZero = false; } if(bZero) set_velocity(SmartPtr<CplUserData<MathVector<dim>, dim> >()); else set_velocity(SmartPtr<ConstUserVector<dim> >(new ConstUserVector<dim>(vVel))); }
inline void set_velocities(const rvec v_in[]) { int i = 0; for (std::vector<int>::iterator i_atom = indices_.begin(); i_atom != indices_.begin(); ++i_atom, ++i) { set_velocity(i, v_in[*i_atom]); } }
void test_physics(){ printf("testing physics\n"); world_handle hello_world = new_world(); //tests movement, set/get velocity, set/get location(position) po_vector center; center.x = 1.0; center.y = 1.0; po_circle circle2 = create_circ(center, 1.0); po_geometry geo_circle2 = create_geom_circ(circle2, 1.0); po_handle circle02 = add_object (hello_world, &geo_circle2, 20.0, 1.0, 0.0); po_vector location = get_position(circle02); center.x = 0.0; center.y = 0.0; po_circle circle1 = create_circ(center, 5.0); po_geometry geo_circle1 = create_geom_circ(circle1, 1.0); po_handle circle01 = add_object (hello_world, &geo_circle1, 50.0, 0.0, 0.0); set_velocity(circle02,-1.0,0.0); po_vector velocity = get_velocity(circle02); update(hello_world,3); po_vector pos02 = get_position(circle02); po_vector pos01 = get_position(circle01); assert(pos02.x == 17.0); assert(pos02.y == 1.0); assert(pos01.x == 50.0); assert(pos01.y == 0.0); set_location(circle01, 0.0, 0.0); set_velocity(circle01, -7.76,1.7); set_location(circle02, 20.0, 0.0); set_velocity(circle02, -9.0,2.0); update(hello_world,1); po_vector pos002 = get_position(circle02); po_vector pos001 = get_position(circle01); assert( pos002.x == 11.0); assert( pos002.y == 2.0); assert(fabs(pos001.x - -7.76) <= EPSILON); assert(fabs(pos001.y - 1.7)<= EPSILON); }
int16_t main(void) { init_clock(); init_timer(); init_ui(); init_pin(); init_spi(); init_oc(); init_md(); // Current measurement pin pin_analogIn(&A[0]); // SPI pin setup ENC_MISO = &D[1]; ENC_MOSI = &D[0]; ENC_SCK = &D[2]; ENC_NCS = &D[3]; pin_digitalOut(ENC_NCS); pin_set(ENC_NCS); // Open SPI in mode 1 spi_open(&spi1, ENC_MISO, ENC_MOSI, ENC_SCK, 2e6, 1); // Motor setup md_velocity(&md1, 0, 0); // Get initial angle offset uint8_t unset = 1; while (unset) { ANG_OFFSET = enc_readReg((WORD) REG_ANG_ADDR); unset = parity(ANG_OFFSET.w); } ANG_OFFSET.w &= ENC_MASK; // USB setup InitUSB(); while (USB_USWSTAT!=CONFIG_STATE) { ServiceUSB(); } // Timers timer_setFreq(&timer2, READ_FREQ); timer_setFreq(&timer3, CTRL_FREQ); timer_start(&timer2); timer_start(&timer3); // Main loop while (1) { ServiceUSB(); if (timer_flag(&timer2)) { timer_lower(&timer2); get_readings(); } if (timer_flag(&timer3)) { timer_lower(&timer3); set_velocity(); } } }
inline void set_phase_space(const rvec x_in[], const rvec v_in[]) { int i = 0; for (std::vector<int>::iterator i_atom = indices_.begin(); i_atom != indices_.begin(); ++i_atom, ++i) { set_position(i, x_in[*i_atom]); set_velocity(i, v_in[*i_atom]); } }
bool start_seeking(int dst) { _destination = dst; if( dst == _sense.position() ) { return start_centering(); } mav(_motor, -150); sleep(0.1); mav(_motor, 0); _state = seeking; if( dst > _sense.position() ) set_velocity(Cart::_max_velocity); else set_velocity(-Cart::_max_velocity); return false; }
void PlayerCar::left() { glm::vec3 prev_direction = direction_; direction_ += (0.001f * 10 * glm::normalize(glm::cross(direction_, up_direction_))); direction_ = glm::normalize(direction_); rotation_.Rotate(up_direction_, -1 * acosf(glm::dot(prev_direction, direction_)) * 360.0f / (2.0f*3.1415f)); set_velocity(glm::normalize(get_velocity() + direction_ * 0.001f) * glm::length(get_velocity())); }
/* A step in the movement loop, returns the state of this cart. */ bool step() { switch(_state) { case seeking: _sense.step( direction() ); /* well, there's a gap in my encoder between column 0 and 1. I didn't think it would be a pain in the neck, but it is. */ if( _sense.position() == _destination ) { if( (_motor != motor_shoulder) || !_sense.is_open(1) || _destination != 1 || direction() < 0) { _sense._position = _destination; return start_centering(); } else set_velocity(direction() * _slow_velocity); } return false; case centering: { int sense_state = _sense.is_centered(); if( sense_state == 0 ) { set_velocity(0); _state = done; return true; } else { if( sense_state > 0 ) set_velocity( Cart::_slow_velocity ); else set_velocity( -Cart::_slow_velocity ); } return false; } case done: return true; } return true; }
void meteorite_t::on_collide(physic_element_if *other) { if (!is_descedant) { auto parent = get_parent(); if (parent) { auto descedant_1 = parent->add_child(new meteorite_t(_position, _size / 2, get_world())); auto descedant_2 = parent->add_child(new meteorite_t(_position, _size / 2, get_world())); descedant_1->is_descedant = true; descedant_2->is_descedant = true; vec2 vel_1 = vec2(mat3::rotation(30 * M_PI / 180) * (vec3(get_velocity(), 0))); vec2 vel_2 = vec2(mat3::rotation(-30 * M_PI / 180) * (vec3(get_velocity(), 0))); descedant_1->set_velocity(vel_1); descedant_2->set_velocity(vel_2); } } this->destroy(); }
/** * 更新 * */ void Girl::update() { if ( mode_ == MODE_FLOAT ) { set_velocity( Vector( 0.f, 0.f, 0.f ) ); update_velocity_by_flicker( flicker_base_location_, get_flicker_scale() ); } /* play_animation( "Float", true, true ); get_animation_player()->set_speed( 0.5f ); chase_direction_to( player_->get_location(), 1.f ); */ }
void PbfSolver::ImposeBoundaryConstraint_() { const vec_t world_sz_dim{world_size_x_, world_size_y_, world_size_z_}; for (size_t ptc_i = 0; ptc_i < ps_->NumParticles(); ++ptc_i) { auto ptc = ps_->Get(ptc_i); auto pos = ptc.position(); auto vel = ptc.velocity(); for (int c = 0; c < 3; ++c) { if (pos[c] <= 0.0f || (pos[c] >= world_sz_dim[c] - kFloatEpsilon)) { vel[c] = 0.0f; pos[c] = std::max(0.0f, std::min(world_sz_dim[c] - kFloatEpsilon, pos[c])); } } ptc.set_position(pos); ptc.set_velocity(vel); } }
/* Makes the robot turn left */ void BasicPlayer :: slow_turn_left() { set_velocity(-MR_SLOW_WHEEL_SPEED, MR_SLOW_WHEEL_SPEED); }
/* Makes the robot turn right */ void BasicPlayer :: turn_right() { set_velocity(MR_NORMAL_WHEEL_SPEED, -MR_NORMAL_WHEEL_SPEED); }
/* Makes the robot turn left */ void BasicPlayer :: turn_left() { set_velocity(-MR_NORMAL_WHEEL_SPEED, MR_NORMAL_WHEEL_SPEED); }
/* Makes the robot turn right */ void BasicPlayer :: quick_turn_right() { set_velocity(MR_FASTEST_WHEEL_SPEED, -MR_FASTEST_WHEEL_SPEED); }
/* Makes the robot turn left */ void BasicPlayer :: quick_turn_left() { set_velocity(-MR_FASTEST_WHEEL_SPEED, MR_FASTEST_WHEEL_SPEED); }
/* Stops the robot */ void BasicPlayer :: stop() { set_velocity(0,0); }
/* Makes the robot go straight with same * velocity for both wheels. */ void BasicPlayer :: go_straight(double speed) { set_velocity(speed,speed); }
/* Makes the robot turn right */ void BasicPlayer :: slow_turn_right() { set_velocity(MR_SLOW_WHEEL_SPEED,-MR_SLOW_WHEEL_SPEED); }
void SimpleBody::preupdate() { set_velocity(0,0,0); }
void set_velocity(float xvel, float yvel) { set_velocity(Vector(xvel, yvel)); }
/* Makes the robot go in a curve with a * trajectory of an arc to the right */ void BasicPlayer :: curve_right() { set_velocity(MR_FASTEST_WHEEL_SPEED,MR_SLOW_WHEEL_SPEED); }
void ConvectionDiffusionBase<TDomain>:: set_velocity(const char* fctName) { set_velocity(LuaUserDataFactory<MathVector<dim>,dim>::create(fctName)); }
void ConvectionDiffusionBase<TDomain>:: set_velocity(LuaFunctionHandle fct) { set_velocity(make_sp(new LuaUserData<MathVector<dim>,dim>(fct))); }
DSC::VelocityFunc::VelocityFunc(double velocity, double accuracy, int max_time_steps) : MAX_TIME_STEPS(max_time_steps) { set_velocity(velocity); set_accuracy(accuracy); }