diff --git a/src/math.hpp b/src/math.hpp index 0f6a8f1..9b9e9db 100644 --- a/src/math.hpp +++ b/src/math.hpp @@ -9,7 +9,6 @@ typedef struct vec2 { float x, y; } vec2; typedef struct vec2i { int x, y; } vec2i; -typedef struct polar_t { float angle, length; } polar_t; static const vec2 vec2_zero = { 0, 0 }; static const vec2 vec2_one = { 1, 1 }; @@ -103,29 +102,30 @@ static vec2 max2(vec2 const &a, vec2 const &b) static vec2 polar_to_vec2(const float angle, const float len) { - // 0 (north) up + // 0 (north) up, PI/2 (east) right return { .x = len * sinf(angle), .y = len * cosf(angle) }; } -static polar_t vec2_to_polar(const vec2 v) +static float vec2_angle(const vec2 v) { - // 0 (north) up - if (v.x == 0) - return { - .angle = (v.y > 0) ? 0 : M_PIf, - .length = abs(v.y) - }; - - polar_t ret = { - .angle = atanf(v.x / v.y) + (v.y > 0 ? 0 : M_PIf), - .length = norm(v) - }; - - if (ret.angle < 0) - ret.angle += 2 * M_PIf; - - return ret; + return atan2f(v.x, v.y); +} + +static float normalize_angle(const float angle) +{ + float a = angle; + while (a > 2*M_PIf) a -= 2*M_PIf; + while (a < 0) a += 2*M_PIf; + return a; +} + +static float normalize_angle_diff(const float diff) +{ + float d = diff; + while (d > M_PIf) d -= 2*M_PIf; + while (d < -M_PIf) d += 2*M_PIf; + return d; } diff --git a/src/sim/aircraft.cpp b/src/sim/aircraft.cpp index 5d310a4..146f3e0 100644 --- a/src/sim/aircraft.cpp +++ b/src/sim/aircraft.cpp @@ -5,16 +5,13 @@ void dxd::sim::Aircraft::tick(float timestep, World *world) { (void)world; - float target_direction = vec2_to_polar(_target - _position).angle; - float target_deviation = target_direction - _direction; - //TODO: Fix this normalization mess - if (target_deviation > M_PIf) - { - target_deviation -= 2 * M_PIf; - } - printf("%0.2f %.2f %0.2f\n", target_direction, target_deviation, target_direction - _direction); + float target_angle = vec2_angle(_target - _position); + float target_deviation = target_angle - _direction; + + target_deviation = normalize_angle_diff(target_deviation); _direction += SDL_clamp(target_deviation, -_MAX_TURN_RATE, _MAX_TURN_RATE) * timestep; + _direction = normalize_angle(_direction); _position = _position + polar_to_vec2(_direction, _speed) * timestep; if (--_trail_cooldown == 0) @@ -29,4 +26,7 @@ void dxd::sim::Aircraft::draw(Renderer *rend) rend->color(255, 255, 255, 255); rend->box(_position, 1); rend->line(_position, _position + polar_to_vec2(_direction, 3)); + + rend->color(255, 64, 64, 255); + rend->diamond(_target, 1); } diff --git a/src/sim/airfield.cpp b/src/sim/airfield.cpp index 595eacb..61f1987 100644 --- a/src/sim/airfield.cpp +++ b/src/sim/airfield.cpp @@ -15,7 +15,7 @@ void dxd::sim::Airfield::tick(float timestep, World *world) { vec2 pos = _position + polar_to_vec2(_rw_heading, _WR_LENGTH+0.5); //vec2 tgt = _position + polar_to_vec2(SDL_randf()*2*M_PIf, 30); - vec2 tgt = _position + polar_to_vec2(0, 80); + vec2 tgt = _position + polar_to_vec2(0, 30); Aircraft *af = new Aircraft(pos, _rw_heading, 5.0f, tgt); world->add_obj(af); //_next_spawn = _MIN_TAKEOFF_DELAY + SDL_rand(_MAX_TAKEOFF_DELAY - _MIN_TAKEOFF_DELAY);