/*
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;
}
}
