Ejemplo n.º 1
0
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);
  }
}
Ejemplo n.º 2
0
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);
}
Ejemplo n.º 3
0
  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)));
}
Ejemplo n.º 5
0
 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]);
   }
 }
Ejemplo n.º 6
0
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);
}
Ejemplo n.º 7
0
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();
        }
    }
}
Ejemplo n.º 8
0
 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]);
   }
 }
Ejemplo n.º 9
0
  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;
  }
Ejemplo n.º 10
0
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()));
}
Ejemplo n.º 11
0
  /* 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;
  }
Ejemplo n.º 12
0
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();
}
Ejemplo n.º 13
0
/**
 * 更新
 *
 */
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 );
	*/
}
Ejemplo n.º 14
0
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);
  }
}
Ejemplo n.º 15
0
/* Makes the robot turn left
 */
void
BasicPlayer :: slow_turn_left()
{
    set_velocity(-MR_SLOW_WHEEL_SPEED, MR_SLOW_WHEEL_SPEED);
}
Ejemplo n.º 16
0
/* Makes the robot turn right
 */
void
BasicPlayer :: turn_right()
{
    set_velocity(MR_NORMAL_WHEEL_SPEED, -MR_NORMAL_WHEEL_SPEED);
}
Ejemplo n.º 17
0
/* Makes the robot turn left
 */
void
BasicPlayer :: turn_left()
{
    set_velocity(-MR_NORMAL_WHEEL_SPEED, MR_NORMAL_WHEEL_SPEED);
}
Ejemplo n.º 18
0
/* Makes the robot turn right
 */
void
BasicPlayer :: quick_turn_right()
{
    set_velocity(MR_FASTEST_WHEEL_SPEED, -MR_FASTEST_WHEEL_SPEED);
}
Ejemplo n.º 19
0
/* Makes the robot turn left
 */
void
BasicPlayer :: quick_turn_left()
{
    set_velocity(-MR_FASTEST_WHEEL_SPEED, MR_FASTEST_WHEEL_SPEED);
}
Ejemplo n.º 20
0
/* Stops the robot
 */
void
BasicPlayer :: stop()
{
    set_velocity(0,0);
}
Ejemplo n.º 21
0
/* Makes the robot go straight with same
 * velocity for both wheels.
 */
void
BasicPlayer :: go_straight(double speed)
{
    set_velocity(speed,speed);
}
Ejemplo n.º 22
0
/* Makes the robot turn right
 */
void
BasicPlayer :: slow_turn_right()
{
    set_velocity(MR_SLOW_WHEEL_SPEED,-MR_SLOW_WHEEL_SPEED);
}
Ejemplo n.º 23
0
void SimpleBody::preupdate() { set_velocity(0,0,0); }
Ejemplo n.º 24
0
		void set_velocity(float xvel, float yvel) { set_velocity(Vector(xvel, yvel)); }
Ejemplo n.º 25
0
/* 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)));
}
Ejemplo n.º 28
0
DSC::VelocityFunc::VelocityFunc(double velocity, double accuracy, int max_time_steps) : MAX_TIME_STEPS(max_time_steps) {
    set_velocity(velocity);
    set_accuracy(accuracy);
}