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);
}
