Interception.cpp

#include <cstdio>
#include <math.h>
#include <unistd.h>
#include <cstdlib>
#include <ctime>
#define _USE_MATH_DEFINES
#define RAD_TO_DEG(x) (x)*(180.0/M_PI)
#define DEG_TO_RAD(x) (x)*(M_PI/180.0)

struct aircraft
{
    float x_pos;
    float y_pos;
    float spd;
    float hdg;
};

int main()
{
    aircraft bomber;
    aircraft fighter;

    srand(time(NULL));

    bomber.spd = 20.0f;
    bomber.x_pos = float((rand() % 1000) - 500); // 200.0f;
    bomber.y_pos = float((rand() % 1000) - 500); // 350.0f;
    bomber.hdg = 6.00f;
    fighter.spd = 25.5f;
    fighter.x_pos = float((rand() % 1000) - 500); // 0.0f;
    fighter.y_pos = float((rand() % 1000) - 500); // 0.0f;

    printf("\nBomber X: %.1f\n", bomber.x_pos);
    printf("Bomber Y: %.1f\n", bomber.y_pos);
    printf("Bomber Hdg: %.2f\n", bomber.hdg);
    printf("Fighter X: %.1f\n", fighter.x_pos);
    printf("Fighter Y: %.1f\n\n", fighter.y_pos);
    float x_dist = fighter.x_pos - bomber.x_pos;
    float y_dist = fighter.y_pos - bomber.y_pos;
    float tot_dist = hypotf(y_dist, x_dist);
    printf("Total Distance: %.2f\n\n", tot_dist);

    float bmb2fgt_brg = atan2f(y_dist, x_dist);
    printf("Bearing bmb to fgt: %.2f\n", bmb2fgt_brg);
    float fgt2bmb_brg = atan2f(-y_dist, -x_dist);
    printf("Bearing fgt to bmb: %.2f\n\n", fgt2bmb_brg);

    // Correct to full rads. It helps.
    if(bmb2fgt_brg < 0.0f)
        bmb2fgt_brg += (M_PI * 2.0f);
    if(fgt2bmb_brg < 0.0f)
        fgt2bmb_brg += (M_PI * 2.0f);

    printf("Bearing bmb to fgt (corrected): %.2f\n", bmb2fgt_brg);
    printf("Bearing fgt to bmb (corrected): %.2f\n\n", fgt2bmb_brg);

    // Relative bearing (angle on bow) bearing - heading;
    float bmb_ang_bow = bmb2fgt_brg - bomber.hdg;
    // Correct so that relative bearings are 0.0 to -3.14159 (left) or 0.0 to 3.14159 (right).
    if(bmb_ang_bow < -M_PI)
        bmb_ang_bow += (M_PI * 2);
    if(bmb_ang_bow > M_PI)
        bmb_ang_bow -= (M_PI * 2);

    printf("bmb to fgt angle on bow: %.2f\n\n", bmb_ang_bow);

    // Get the angle to intercept for the fighter.
    float sine_angle = (bomber.spd * sinf(bmb_ang_bow)) / fighter.spd;
    printf("Sine of angle to intercept: %.2f\n\n", sine_angle);
    float inter_angle = asinf(sine_angle);
    printf("Angle to intercept (radians): %.2f\n", inter_angle);
    printf("Angle to intercept (degrees): %.2f\n\n", RAD_TO_DEG(inter_angle));

    float inter_hdg = fgt2bmb_brg + inter_angle;
    if(inter_hdg > (M_PI * 2))
        inter_hdg -= (M_PI * 2);
    if(inter_hdg < 0.0f)
        inter_hdg += (M_PI * 2);

    printf("Heading to intercept (radians): %.2f\n", inter_hdg);
    printf("Heading to intercept (degrees): %.2f\n\n", RAD_TO_DEG(inter_hdg));

    fighter.hdg = inter_hdg;

    // Get the convergence angle. We will need this to calculate distance to intercept.
    // We will need the intercept angle and the angle on bow. As this is a pure Sine Rule
    // problem, do not accept any negative values, if only for convenience. You are going to
    // need to compute the remaining (convergence) angle.
    float int_angle_mod = inter_angle;
    float bmb_ang_bow_mod = bmb_ang_bow;
    if(int_angle_mod < 0.0f)
        int_angle_mod *= -1.0f;
    if(bmb_ang_bow_mod < 0.0f)
        bmb_ang_bow_mod *= -1.0f;

    float converge_angle = M_PI - (int_angle_mod + bmb_ang_bow_mod);

    // A bit extra here. Calculate the range at which the interception is going to happen,
    // using the Sine Rule.
    float inter_range = (tot_dist / sinf(converge_angle)) * sinf(bmb_ang_bow_mod);
    bool proceed = false;
    if(inter_range > 0.0f)
    {
        printf("Distance to intercept %.2f\n\n", inter_range);
        proceed = true;
    }
    if(inter_range < 0.0f)
        printf("Intercept not possible.\n\n");

    usleep(3000000);
    while(proceed == true)
    {
        usleep(75000);
        bomber.x_pos += cosf(bomber.hdg) * bomber.spd;
        bomber.y_pos += sinf(bomber.hdg) * bomber.spd;
        fighter.x_pos += cosf(fighter.hdg) * fighter.spd;
        fighter.y_pos += sinf(fighter.hdg) * fighter.spd;
        x_dist = fighter.x_pos - bomber.x_pos;
        y_dist = fighter.y_pos - bomber.y_pos;
        tot_dist = hypotf(y_dist, x_dist);
        printf("Closure distance: %.1f\n", tot_dist);
        if(tot_dist < 25.0f)
        {
            printf("\nINTERCEPTED!\n\n");
            proceed = false;
        }
    }

    return 0;
}