/* update the simulation attitude and relative position */ void Aircraft::update_dynamics(const Vector3f &rot_accel) { float delta_time = frame_time_us * 1.0e-6f; // update rotational rates in body frame gyro += rot_accel * delta_time; gyro.x = constrain_float(gyro.x, -radians(2000), radians(2000)); gyro.y = constrain_float(gyro.y, -radians(2000), radians(2000)); gyro.z = constrain_float(gyro.z, -radians(2000), radians(2000)); // update attitude dcm.rotate(gyro * delta_time); dcm.normalize(); Vector3f accel_earth = dcm * accel_body; accel_earth += Vector3f(0, 0, GRAVITY_MSS); // if we're on the ground, then our vertical acceleration is limited // to zero. This effectively adds the force of the ground on the aircraft if (on_ground(position) && accel_earth.z > 0) { accel_earth.z = 0; } // work out acceleration as seen by the accelerometers. It sees the kinematic // acceleration (ie. real movement), plus gravity accel_body = dcm.transposed() * (accel_earth + Vector3f(0, 0, -GRAVITY_MSS)); // new velocity vector velocity_ef += accel_earth * delta_time; // new position vector Vector3f old_position = position; position += velocity_ef * delta_time; // velocity relative to air mass, in earth frame velocity_air_ef = velocity_ef - wind_ef; // velocity relative to airmass in body frame velocity_air_bf = dcm.transposed() * velocity_air_ef; // airspeed airspeed = velocity_air_ef.length(); // airspeed as seen by a fwd pitot tube (limited to 120m/s) airspeed_pitot = constrain_float(velocity_air_bf * Vector3f(1, 0, 0), 0, 120); // constrain height to the ground if (on_ground(position)) { if (!on_ground(old_position) && AP_HAL::millis() - last_ground_contact_ms > 1000) { printf("Hit ground at %f m/s\n", velocity_ef.z); last_ground_contact_ms = AP_HAL::millis(); } position.z = -(ground_level + frame_height - home.alt*0.01f); } }
void CaptainSnowball::active_update(float elapsed_time) { if (on_ground() && might_climb(8, 64)) { physic.set_velocity_y(-400); } else if (on_ground() && might_fall(16)) { physic.set_velocity_y(-400); walk_speed = BOARDING_SPEED; physic.set_velocity_x(dir == LEFT ? -walk_speed : walk_speed); } WalkingBadguy::active_update(elapsed_time); }
void Hedgehog::update(float delta) { if (state == DYING) { if (die_sprite.is_finished()) remove(); die_sprite.update(delta); } else { sprite.update(delta); bool was_on_ground = false; if (on_ground()) { was_on_ground = true; if (velocity.y > 0) { velocity.y = 0; pos.y = int(pos.y / TILE_SIZE) * TILE_SIZE + TILE_SIZE - 1; } if (direction_left) velocity.x = -32; else velocity.x = 32; } else { velocity.y += GRAVITY * delta; } Vector old_pos = pos; pos += velocity * delta; if ((was_on_ground && !on_ground()) || in_wall()) { direction_left = !direction_left; pos = old_pos; } } // Check if the player got hit // FIXME: Insert pixel perfect collision detection here Vector player_pos = Player::current()->get_pos(); if (pos.x - 20 < player_pos.x && pos.x + 20 > player_pos.x && pos.y - 20 < player_pos.y && pos.y + 5 > player_pos.y) Player::current()->hit(5); }
void Player::apply_friction() { if ((on_ground()) && (fabs(physic.get_velocity_x()) < WALK_SPEED)) { physic.set_velocity_x(0); physic.set_acceleration_x(0); } else if(physic.get_velocity_x() < 0) { physic.set_acceleration_x(WALK_ACCELERATION_X * 1.5); } else { physic.set_acceleration_x(WALK_ACCELERATION_X * -1.5); } #if 0 // if we're on ice slow down acceleration or deceleration if (isice(base.x, base.y + base.height)) { /* the acceleration/deceleration rate on ice is inversely proportional to * the current velocity. */ // increasing 1 will increase acceleration/deceleration rate // decreasing 1 will decrease acceleration/deceleration rate // must stay above zero, though if (ax != 0) ax *= 1 / fabs(vx); } #endif }
void Yeti::active_update(float elapsed_time) { switch(state) { case JUMP_DOWN: physic.set_velocity_x((dir==RIGHT)?+JUMP_DOWN_VX:-JUMP_DOWN_VX); break; case RUN: physic.set_velocity_x((dir==RIGHT)?+RUN_VX:-RUN_VX); if (((dir == RIGHT) && (get_pos().x >= right_jump_x)) || ((dir == LEFT) && (get_pos().x <= left_jump_x))) jump_up(); break; case JUMP_UP: physic.set_velocity_x((dir==RIGHT)?+JUMP_UP_VX:-JUMP_UP_VX); if (((dir == RIGHT) && (get_pos().x >= right_stand_x)) || ((dir == LEFT) && (get_pos().x <= left_stand_x))) be_angry(); break; case BE_ANGRY: if(state_timer.check() && on_ground()) { physic.set_velocity_y(STOMP_VY); sprite->set_action((dir==RIGHT)?"stomp-right":"stomp-left"); } break; case SQUISHED: if (state_timer.check()) { remove_me(); } break; } movement = physic.get_movement(elapsed_time); }
void Player::handle_vertical_input() { // Press jump key if(controller->pressed(Controller::JUMP)) jump_button_timer.start(JUMP_GRACE_TIME); if (((controller->hold(Controller::JUMP) && jump_button_timer.started()) || jump_helper_jump) && can_jump) { jump_button_timer.stop(); if (duck) { // when running, only jump a little bit; else do a backflip if ((physic.get_velocity_x() != 0) || (controller->hold(Controller::LEFT)) || (controller->hold(Controller::RIGHT))) { do_jump(-300); } else { do_backflip(); } } else { // jump a bit higher if we are running; else do a normal jump if (fabs(physic.get_velocity_x()) > MAX_WALK_XM) do_jump(-580); else do_jump(-520); } } // Let go of jump key else if(!(controller->hold(Controller::JUMP) || jump_helper_jump)) { if (!backflipping && jumping && physic.get_velocity_y() < 0) { jumping = false; early_jump_apex(); } } if(jump_early_apex && physic.get_velocity_y() >= 0) { do_jump_apex(); } /* In case the player has pressed Down while in a certain range of air, enable butt jump action */ if (controller->hold(Controller::DOWN) && !duck && is_big() && !on_ground()) { wants_buttjump = true; if (physic.get_velocity_y() >= BUTTJUMP_MIN_VELOCITY_Y) does_buttjump = true; } /* When Down is not held anymore, disable butt jump */ if(!controller->hold(Controller::DOWN)) { wants_buttjump = false; does_buttjump = false; } // swimming physic.set_acceleration_y(0); #ifdef SWIMMING if (swimming) { if (controller->hold(Controller::UP) || controller->hold(Controller::JUMP)) physic.set_acceleration_y(-2000); physic.set_velocity_y(physic.get_velocity_y() * 0.94); } #endif }
void WalkingBadguy::active_update(float elapsed_time, float dest_x_velocity) { BadGuy::active_update(elapsed_time); float current_x_velocity = physic.get_velocity_x (); if (frozen) { physic.set_velocity_x (0.0); physic.set_acceleration_x (0.0); } /* We're very close to our target speed. Just set it to avoid oscillation */ else if ((current_x_velocity > (dest_x_velocity - 5.0)) && (current_x_velocity < (dest_x_velocity + 5.0))) { physic.set_velocity_x (dest_x_velocity); physic.set_acceleration_x (0.0); } /* Check if we're going too slow or even in the wrong direction */ else if (((dest_x_velocity <= 0.0) && (current_x_velocity > dest_x_velocity)) || ((dest_x_velocity > 0.0) && (current_x_velocity < dest_x_velocity))) { /* acceleration == walk-speed => it will take one second to get from zero * to full speed. */ physic.set_acceleration_x (dest_x_velocity); } /* Check if we're going too fast */ else if (((dest_x_velocity <= 0.0) && (current_x_velocity < dest_x_velocity)) || ((dest_x_velocity > 0.0) && (current_x_velocity > dest_x_velocity))) { /* acceleration == walk-speed => it will take one second to get twice the * speed to normal speed. */ physic.set_acceleration_x ((-1.f) * dest_x_velocity); } else { /* The above should have covered all cases. */ assert (23 == 42); } if (max_drop_height > -1) { if (on_ground() && might_fall(max_drop_height+1)) { turn_around(); } } if ((dir == LEFT) && (physic.get_velocity_x () > 0.0)) { dir = RIGHT; set_action (walk_right_action, /* loops = */ -1); } else if ((dir == RIGHT) && (physic.get_velocity_x () < 0.0)) { dir = LEFT; set_action (walk_left_action, /* loops = */ -1); } }
void Player::on_update(float dt) { auto characterMovement = this->get_world().get_object(this->character_movement); if (characterMovement->on_ground()) { this->_num_jumps = 0; } // Stupid, need to figure out a better way of solving this this->STUPID_update_children_physics_transform(); }
bool Snail::collision_squished(GameObject& object) { if (m_frozen) return WalkingBadguy::collision_squished(object); Player* player = dynamic_cast<Player*>(&object); if (player && (player->m_does_buttjump || player->is_invincible())) { kill_fall(); player->bounce(*this); return true; } switch (state) { case STATE_KICKED: case STATE_NORMAL: // Can't stomp in midair if (!on_ground()) break; squishcount++; if (squishcount >= MAX_SNAIL_SQUISHES) { kill_fall(); return true; } SoundManager::current()->play("sounds/stomp.wav", get_pos()); be_flat(); break; case STATE_FLAT: SoundManager::current()->play("sounds/kick.wav", get_pos()); { MovingObject* movingobject = dynamic_cast<MovingObject*>(&object); if (movingobject && (movingobject->get_pos().x < get_pos().x)) { m_dir = Direction::RIGHT; } else { m_dir = Direction::LEFT; } } be_kicked(); break; case STATE_GRABBED: case STATE_KICKED_DELAY: break; } if (player) player->bounce(*this); return true; }
void Bomb::active_update(float elapsed_time) { if (on_ground()) physic.set_velocity_x(0); ticking->set_position(get_pos()); if(sprite->animation_done()) { explode(); } else if (!grabbed) { movement = physic.get_movement(elapsed_time); } }
void Player::do_backflip() { if (!duck) return; if (!on_ground()) return; backflip_direction = (dir == LEFT)?(+1):(-1); backflipping = true; do_jump(-580); SoundManager::current()->play("sounds/flip.wav"); backflip_timer.start(TUX_BACKFLIP_TIME); }
void WalkingBadguy::active_update(float elapsed_time) { BadGuy::active_update(elapsed_time); if (max_drop_height > -1) { if (on_ground() && might_fall(max_drop_height+1)) { turn_around(); } } }
void Yeti::active_update(float elapsed_time) { switch(state) { case JUMP_DOWN: physic.set_velocity_x((dir==RIGHT)?+JUMP_DOWN_VX:-JUMP_DOWN_VX); break; case RUN: physic.set_velocity_x((dir==RIGHT)?+RUN_VX:-RUN_VX); if (((dir == RIGHT) && (get_pos().x >= right_jump_x)) || ((dir == LEFT) && (get_pos().x <= left_jump_x))) jump_up(); break; case JUMP_UP: physic.set_velocity_x((dir==RIGHT)?+JUMP_UP_VX:-JUMP_UP_VX); if (((dir == RIGHT) && (get_pos().x >= right_stand_x)) || ((dir == LEFT) && (get_pos().x <= left_stand_x))) be_angry(); break; case BE_ANGRY: if(state_timer.check() && on_ground()) { physic.set_velocity_y(STOMP_VY); sprite->set_action((dir==RIGHT)?"stomp-right":"stomp-left"); SoundManager::current()->play("sounds/yeti_gna.wav"); } break; case SQUISHED: { Direction newdir = (int(state_timer.get_timeleft() * SNOW_EXPLOSIONS_FREQUENCY) % 2) ? LEFT : RIGHT; if (dir != newdir && dir == RIGHT) { SoundManager::current()->play("sounds/stomp.wav"); add_snow_explosions(); Sector::current()->camera->shake(.05f, 0, 5); } dir = newdir; sprite->set_action((dir==RIGHT)?"jump-right":"jump-left"); } if (state_timer.check()) { BadGuy::kill_fall(); state = FALLING; physic.set_velocity_y(JUMP_UP_VY / 2); // Move up a bit before falling // Add some extra explosions for (int i = 0; i < 10; i++) { add_snow_explosions(); } run_dead_script(); } break; case FALLING: break; } movement = physic.get_movement(elapsed_time); }
void Totem::active_update(float elapsed_time) { BadGuy::active_update(elapsed_time); if (!carried_by) { if (on_ground() && might_fall()) { dir = (dir == LEFT ? RIGHT : LEFT); initialize(); } Sector* s = Sector::current(); if (s) { // jump a bit if we find a suitable totem for (std::vector<MovingObject*>::iterator i = s->moving_objects.begin(); i != s->moving_objects.end(); ++i) { Totem* t = dynamic_cast<Totem*>(*i); if (!t) continue; // skip if we are not approaching each other if (!((dir == LEFT) && (t->dir == RIGHT))) continue; Vector p1 = bbox.p1; Vector p2 = t->get_pos(); // skip if not on same height float dy = (p1.y - p2.y); if (fabsf(dy - 0) > 2) continue; // skip if too far away float dx = (p1.x - p2.x); if (fabsf(dx - 128) > 2) continue; physic.set_velocity_y(JUMP_ON_SPEED_Y); p1.y -= 1; this->set_pos(p1); break; } } } if (carried_by) { this->synchronize_with(carried_by); } if (carrying) { carrying->synchronize_with(this); } }
void Player::do_backflip() { if (!duck) return; if (!on_ground()) return; // TODO: we don't have an animation for firetux backflipping, so let's revert to bigtux set_bonus(GROWUP_BONUS, true); backflip_direction = (dir == LEFT)?(+1):(-1); backflipping = true; do_jump(-580); sound_manager->play("sounds/flip.wav"); backflip_timer.start(0.15); }
void Player::handle_input() { if (ghost_mode) { handle_input_ghost(); return; } if(!controller->hold(Controller::ACTION) && grabbed_object) { // move the grabbed object a bit away from tux Vector pos = get_pos() + Vector(dir == LEFT ? -bbox.get_width()-1 : bbox.get_width()+1, bbox.get_height()*0.66666 - 32); Rect dest(pos, pos + Vector(32, 32)); if(Sector::current()->is_free_space(dest)) { MovingObject* moving_object = dynamic_cast<MovingObject*> (grabbed_object); if(moving_object) { moving_object->set_pos(pos); } else { log_debug << "Non MovingObjetc grabbed?!?" << std::endl; } grabbed_object->ungrab(*this, dir); grabbed_object = NULL; } } /* Handle horizontal movement: */ if (!backflipping) handle_horizontal_input(); /* Jump/jumping? */ if (on_ground() && !controller->hold(Controller::JUMP)) can_jump = true; /* Handle vertical movement: */ handle_vertical_input(); /* Shoot! */ if (controller->pressed(Controller::ACTION) && player_status->bonus == FIRE_BONUS) { if(Sector::current()->add_bullet( get_pos() + ((dir == LEFT)? Vector(0, bbox.get_height()/2) : Vector(32, bbox.get_height()/2)), physic.get_velocity_x(), dir)) shooting_timer.start(SHOOTING_TIME); } /* Duck or Standup! */ if (controller->hold(Controller::DOWN)) do_duck(); else do_standup(); }
void check_against_map(struct Player *p) { int i; /* move down / fall */ if(p->vy > 0) { for(i = 0; i < p->vy; i++) { on_ground(p); if(!p->on_ground) { p->y += 1; } else { break; } } } /* jump */ if(p->on_ground) { if(p->vy < 0) { for(i = 0; i > p->vy; i--) { if(not_solid_above(p)) { p->y -= 1; } else { /* reset upward movement to prevent jumping if player hits above block and then moves left/right */ p->vy = 0; } } } } /* move right */ if(p->vx > 0) { for(i = 0; i < p->vx; i++) { if((!solid(p->x + p->w, p->y)) && (!solid(p->x + p->w, p->y + p->h/2 - 1)) && (!solid(p->x + p->w, p->y + p->h - 1))) { p->x += 1; } } } /* move left */ if(p->vx < 0) { for(i = 0; i > p->vx; i--) { if((!solid(p->x - 1, p->y)) && !solid(p->x - 1, p->y + p->h/2 - 1) && !solid(p->x - 1, p->y + p->h - 1)) { p->x -= 1; } } } }
void Player::do_duck() { if (duck) return; if (!is_big()) return; if (physic.get_velocity_y() != 0) return; if (!on_ground()) return; if (adjust_height(31.8)) { duck = true; unduck_hurt_timer.stop(); } else { // FIXME: what now? } }
void Player::apply_friction() { if ((on_ground()) && (fabs(physic.get_velocity_x()) < WALK_SPEED)) { physic.set_velocity_x(0); physic.set_acceleration_x(0); } else { float friction = WALK_ACCELERATION_X * (on_ice ? ICE_FRICTION_MULTIPLIER : NORMAL_FRICTION_MULTIPLIER); if(physic.get_velocity_x() < 0) { physic.set_acceleration_x(friction); } else if(physic.get_velocity_x() > 0) { physic.set_acceleration_x(-friction); } // no friction for physic.get_velocity_x() == 0 } }
void update(float delta) { //std::cout << "Delta: " << delta << std::endl; if (!on_ground()) vel_y += 10 * delta; if (jump) vel_y = -5; TTY_SetCursor(tty, 0, 28); TTY_printf(tty, "Velocity: %3.2f %3.2f %d %d \r", vel_x, vel_y, get_tile(x, y), on_ground()); float last_x = x; float last_y = y; x += vel_x; if (clean()) { x = last_x; vel_x = 0; } y += vel_y; if (clean()) { y = last_y; vel_y = 0; } switch(direction) { case LEFT: if (vel_x > -5.0f) vel_x -= 10 * delta; break; case RIGHT: if (vel_x < 5.0f) vel_x += 10 * delta; break; case NONE: vel_x -= vel_x * delta * 10.0f; break; } }
void BadGuy::ignite() { if (!is_flammable() || m_ignited) { return; } m_physic.enable_gravity(true); m_physic.set_velocity_x(0); m_physic.set_velocity_y(0); set_group(COLGROUP_MOVING_ONLY_STATIC); m_sprite->stop_animation(); m_ignited = true; if (m_sprite->has_action("melting-left")) { // melt it! if (m_sprite->has_action("ground-melting-left") && on_ground()) { m_sprite->set_action(m_dir == Direction::LEFT ? "ground-melting-left" : "ground-melting-right", 1); SoundManager::current()->play("sounds/splash.ogg", get_pos()); set_state(STATE_GROUND_MELTING); } else { m_sprite->set_action(m_dir == Direction::LEFT ? "melting-left" : "melting-right", 1); SoundManager::current()->play("sounds/sizzle.ogg", get_pos()); set_state(STATE_MELTING); } run_dead_script(); } else if (m_sprite->has_action("burning-left")) { // burn it! m_glowing = true; SoundManager::current()->play("sounds/fire.ogg", get_pos()); m_sprite->set_action(m_dir == Direction::LEFT ? "burning-left" : "burning-right", 1); set_state(STATE_BURNING); run_dead_script(); } else if (m_sprite->has_action("inside-melting-left")) { // melt it inside! SoundManager::current()->play("sounds/splash.ogg", get_pos()); m_sprite->set_action(m_dir == Direction::LEFT ? "inside-melting-left" : "inside-melting-right", 1); set_state(STATE_INSIDE_MELTING); run_dead_script(); } else { // Let it fall off the screen then. kill_fall(); } }
void Totem::active_update(float dt_sec) { BadGuy::active_update(dt_sec); if (!carried_by) { if (on_ground() && might_fall()) { m_dir = (m_dir == Direction::LEFT ? Direction::RIGHT : Direction::LEFT); initialize(); } // jump a bit if we find a suitable totem for (auto& obj : Sector::get().get_objects_by_type<MovingObject>()) { auto t = dynamic_cast<Totem*>(&obj); if (!t) continue; // skip if we are not approaching each other if (!((m_dir == Direction::LEFT) && (t->m_dir == Direction::RIGHT))) continue; Vector p1 = m_col.m_bbox.p1(); Vector p2 = t->get_pos(); // skip if not on same height float dy = (p1.y - p2.y); if (fabsf(dy - 0) > 2) continue; // skip if too far away float dx = (p1.x - p2.x); if (fabsf(dx - 128) > 2) continue; m_physic.set_velocity_y(JUMP_ON_SPEED_Y); p1.y -= 1; set_pos(p1); break; } } if (carried_by) { synchronize_with(carried_by); } if (carrying) { carrying->synchronize_with(this); } }
void Player::do_jump(float yspeed) { if (!on_ground()) return; physic.set_velocity_y(yspeed); //bbox.move(Vector(0, -1)); jumping = true; on_ground_flag = false; can_jump = false; // play sound if (is_big()) { sound_manager->play("sounds/bigjump.wav"); } else { sound_manager->play("sounds/jump.wav"); } }
void Player::handle_vertical_input() { // Press jump key if(input.up == DOWN && can_jump) { if (on_ground()) { // jump higher if we are running if (fabs(physic.get_velocity_x()) > MAX_WALK_XM) physic.set_velocity_y(5.8); else physic.set_velocity_y(5.2); --base.y; jumping = true; can_jump = false; if (size == SMALL) play_sound(sounds[SND_JUMP], SOUND_CENTER_SPEAKER); else play_sound(sounds[SND_BIGJUMP], SOUND_CENTER_SPEAKER); } } // Let go of jump key else if(input.up == UP && jumping) { jumping = false; if(physic.get_velocity_y() > 0) { physic.set_velocity_y(0); } } if ( (issolid(base.x + base.width / 2, base.y + base.height + 64) || issolid(base.x + 1, base.y + base.height + 64) || issolid(base.x + base.width - 1, base.y + base.height + 64)) && jumping == false && can_jump == false && input.up == DOWN && input.old_up == UP) { can_jump = true; } input.old_up = input.up; }
void GoldBomb::active_update(float elapsed_time) { if(tstate == STATE_TICKING) { if (on_ground()) physic.set_velocity_x(0); ticking->set_position(get_pos()); if(sprite->animation_done()) { kill_fall(); } else if (!grabbed) { movement = physic.get_movement(elapsed_time); } return; } if(grabbed) return; WalkingBadguy::active_update(elapsed_time); }
void Player::do_duck() { if (duck) return; if (!is_big()) return; if (physic.get_velocity_y() != 0) return; if (!on_ground()) return; if (does_buttjump) return; if (adjust_height(DUCKED_TUX_HEIGHT)) { duck = true; growing = false; unduck_hurt_timer.stop(); } else { // FIXME: what now? } }
/* update wind vector */ void Aircraft::update_wind(const struct sitl_input &input) { // wind vector in earth frame wind_ef = Vector3f(cosf(radians(input.wind.direction)), sinf(radians(input.wind.direction)), 0) * input.wind.speed; const float wind_turb = input.wind.turbulence * 10.0; // scale input.wind.turbulence to match standard deviation when using iir_coef=0.98 const float iir_coef = 0.98; // filtering high frequencies from turbulence if (wind_turb > 0 && !on_ground(position)) { turbulence_azimuth = turbulence_azimuth + (2 * rand()); turbulence_horizontal_speed= turbulence_horizontal_speed * iir_coef+wind_turb * rand_normal(0,1) * (1-iir_coef); turbulence_vertical_speed = (turbulence_vertical_speed * iir_coef) + (wind_turb * rand_normal(0,1) * (1-iir_coef)); wind_ef += Vector3f( cosf(radians(turbulence_azimuth)) * turbulence_horizontal_speed, sinf(radians(turbulence_azimuth)) * turbulence_horizontal_speed, turbulence_vertical_speed); } }
/* update the helicopter simulation by one time step */ void Helicopter::update(const struct sitl_input &input) { // get wind vector setup update_wind(input); float rsc = constrain_float((input.servos[7]-1000) / 1000.0f, 0, 1); // ignition only for gas helis bool ignition_enabled = gas_heli?(input.servos[5] > 1500):true; float thrust = 0; float roll_rate = 0; float pitch_rate = 0; float yaw_rate = 0; float torque_effect_accel = 0; float lateral_x_thrust = 0; float lateral_y_thrust = 0; float swash1 = (input.servos[0]-1000) / 1000.0f; float swash2 = (input.servos[1]-1000) / 1000.0f; float swash3 = (input.servos[2]-1000) / 1000.0f; if (!ignition_enabled) { rsc = 0; } float rsc_scale = rsc/rsc_setpoint; switch (frame_type) { case HELI_FRAME_CONVENTIONAL: { // simulate a traditional helicopter float tail_rotor = (input.servos[3]-1000) / 1000.0f; thrust = (rsc/rsc_setpoint) * (swash1+swash2+swash3) / 3.0f; torque_effect_accel = (rsc_scale+thrust) * rotor_rot_accel; roll_rate = swash1 - swash2; pitch_rate = (swash1+swash2) / 2.0f - swash3; yaw_rate = tail_rotor - 0.5f; lateral_y_thrust = yaw_rate * rsc_scale * tail_thrust_scale; break; } case HELI_FRAME_DUAL: { // simulate a tandem helicopter float swash4 = (input.servos[3]-1000) / 1000.0f; float swash5 = (input.servos[4]-1000) / 1000.0f; float swash6 = (input.servos[5]-1000) / 1000.0f; thrust = (rsc / rsc_setpoint) * (swash1+swash2+swash3+swash4+swash5+swash6) / 6.0f; torque_effect_accel = (rsc_scale + rsc / rsc_setpoint) * rotor_rot_accel * ((swash1+swash2+swash3) - (swash4+swash5+swash6)); roll_rate = (swash1-swash2) + (swash4-swash5); pitch_rate = (swash1+swash2+swash3) - (swash4+swash5+swash6); yaw_rate = (swash1-swash2) + (swash5-swash4); break; } case HELI_FRAME_COMPOUND: { // simulate a compound helicopter float right_rotor = (input.servos[3]-1000) / 1000.0f; float left_rotor = (input.servos[4]-1000) / 1000.0f; thrust = (rsc/rsc_setpoint) * (swash1+swash2+swash3) / 3.0f; torque_effect_accel = (rsc_scale+thrust) * rotor_rot_accel; roll_rate = swash1 - swash2; pitch_rate = (swash1+swash2) / 2.0f - swash3; yaw_rate = right_rotor - left_rotor; lateral_x_thrust = (left_rotor+right_rotor-1) * rsc_scale * tail_thrust_scale; break; } } roll_rate *= rsc_scale; pitch_rate *= rsc_scale; yaw_rate *= rsc_scale; // rotational acceleration, in rad/s/s, in body frame Vector3f rot_accel; rot_accel.x = roll_rate * roll_rate_max; rot_accel.y = pitch_rate * pitch_rate_max; rot_accel.z = yaw_rate * yaw_rate_max; // rotational air resistance rot_accel.x -= gyro.x * radians(5000.0) / terminal_rotation_rate; rot_accel.y -= gyro.y * radians(5000.0) / terminal_rotation_rate; rot_accel.z -= gyro.z * radians(400.0) / terminal_rotation_rate; // torque effect on tail rot_accel.z += torque_effect_accel; // air resistance Vector3f air_resistance = -velocity_air_ef * (GRAVITY_MSS/terminal_velocity); // scale thrust to newtons thrust *= thrust_scale; accel_body = Vector3f(lateral_x_thrust, lateral_y_thrust, -thrust / mass); accel_body += dcm * air_resistance; bool was_on_ground = on_ground(position); update_dynamics(rot_accel); // constrain height to the ground if (on_ground(position) && !was_on_ground) { // zero roll/pitch, but keep yaw float r, p, y; dcm.to_euler(&r, &p, &y); dcm.from_euler(0, 0, y); position.z = -(ground_level + frame_height - home.alt*0.01f); velocity_ef.zero(); } // update lat/lon/altitude update_position(); }
/* update the helicopter simulation by one time step */ void Helicopter::update(const struct sitl_input &input) { float swash1 = (input.servos[0]-1000) / 1000.0f; float swash2 = (input.servos[1]-1000) / 1000.0f; float swash3 = (input.servos[2]-1000) / 1000.0f; float tail_rotor = (input.servos[3]-1000) / 1000.0f; float rsc = (input.servos[7]-1000) / 1000.0f; // how much time has passed? float delta_time = frame_time_us * 1.0e-6f; float thrust = (rsc/rsc_setpoint)*(swash1+swash2+swash3)/3.0f; // very simplistic mapping to body euler rates float roll_rate = swash1 - swash2; float pitch_rate = (swash1 + swash2)/2.0f - swash3; float yaw_rate = tail_rotor - 0.5f; float rsc_scale = rsc/rsc_setpoint; roll_rate *= rsc_scale; pitch_rate *= rsc_scale; yaw_rate *= rsc_scale; // rotational acceleration, in rad/s/s, in body frame Vector3f rot_accel; rot_accel.x = roll_rate * roll_rate_max; rot_accel.y = pitch_rate * pitch_rate_max; rot_accel.z = yaw_rate * yaw_rate_max; // rotational air resistance rot_accel.x -= gyro.x * radians(5000.0) / terminal_rotation_rate; rot_accel.y -= gyro.y * radians(5000.0) / terminal_rotation_rate; rot_accel.z -= gyro.z * radians(400.0) / terminal_rotation_rate; // torque effect on tail rot_accel.z += (rsc_scale+thrust) * rotor_rot_accel; // update rotational rates in body frame gyro += rot_accel * delta_time; // update attitude dcm.rotate(gyro * delta_time); dcm.normalize(); // air resistance Vector3f air_resistance = -velocity_ef * (GRAVITY_MSS/terminal_velocity); // scale thrust to newtons thrust *= thrust_scale; accel_body = Vector3f(0, yaw_rate * rsc_scale * tail_thrust_scale, -thrust / mass); Vector3f accel_earth = dcm * accel_body; accel_earth += Vector3f(0, 0, GRAVITY_MSS); accel_earth += air_resistance; // if we're on the ground, then our vertical acceleration is limited // to zero. This effectively adds the force of the ground on the aircraft if (on_ground(position) && accel_earth.z > 0) { accel_earth.z = 0; } // work out acceleration as seen by the accelerometers. It sees the kinematic // acceleration (ie. real movement), plus gravity accel_body = dcm.transposed() * (accel_earth + Vector3f(0, 0, -GRAVITY_MSS)); // add some noise add_noise(thrust / thrust_scale); // new velocity vector velocity_ef += accel_earth * delta_time; // new position vector Vector3f old_position = position; position += velocity_ef * delta_time; // assume zero wind for now airspeed = velocity_ef.length(); // constrain height to the ground if (on_ground(position)) { if (!on_ground(old_position)) { printf("Hit ground at %f m/s\n", velocity_ef.z); velocity_ef.zero(); // zero roll/pitch, but keep yaw float r, p, y; dcm.to_euler(&r, &p, &y); dcm.from_euler(0, 0, y); position.z = -(ground_level + frame_height - home.alt*0.01f); } } // update lat/lon/altitude update_position(); }
void Player::handle_input() { /* Handle horizontal movement: */ handle_horizontal_input(); /* Jump/jumping? */ if (on_ground() && input.up == UP) can_jump = true; if (input.up == DOWN || (input.up == UP && jumping)) { handle_vertical_input(); } /* Shoot! */ if (input.fire == DOWN && input.old_fire == UP && got_coffee) { World::current()->add_bullet(base.x, base.y, physic.get_velocity_x(), dir); input.old_fire = DOWN; } /* tux animations: */ if(!frame_timer.check()) { frame_timer.start(25); if (input.right == UP && input.left == UP) { frame_main = 1; frame_ = 1; } else { if ((input.fire == DOWN && (global_frame_counter % 2) == 0) || (global_frame_counter % 4) == 0) frame_main = (frame_main + 1) % 4; frame_ = frame_main; if (frame_ == 3) frame_ = 1; } } /* Duck! */ if (input.down == DOWN && size == BIG && !duck && physic.get_velocity_y() == 0 && on_ground()) { duck = true; base.height = 32; base.y += 32; // changing base size confuses collision otherwise old_base = previous_base = base; } else if(input.down == UP && size == BIG && duck && physic.get_velocity_y() == 0 && on_ground()) { duck = false; base.y -= 32; base.height = 64; // changing base size confuses collision otherwise old_base = previous_base = base; } }