bgc-c/basic-geometry/vector2.c

#include "vector2.h"

// =================== Angle ==================== //

float bg_fp32_vector2_get_angle(const BgFP32Vector2* vector1, const BgFP32Vector2* vector2, const angle_unit_t unit)
{
    if (vector1 == 0 || vector2 == 0) {
        return 0.0f;
    }

    const float square_module1 = bg_fp32_vector2_get_square_module(vector1);

    if (square_module1 <= BG_FP32_SQUARE_EPSYLON) {
        return 0.0f;
    }

    const float square_module2 = bg_fp32_vector2_get_square_module(vector2);

    if (square_module2 <= BG_FP32_SQUARE_EPSYLON) {
        return 0.0f;
    }

    const float cosine = bg_fp32_vector2_dot_product(vector1, vector2) / sqrtf(square_module1 * square_module2);

    if (cosine >= 1.0f - BG_FP32_EPSYLON) {
        return 0.0f;
    }

    if (cosine <= -1.0f + BG_FP32_EPSYLON) {
        return bg_fp32_angle_get_half_circle(unit);
    }

    return bg_fp32_radians_to_units(acosf(cosine), unit);
}

double bg_fp64_vector2_get_angle(const BgFP64Vector2* vector1, const BgFP64Vector2* vector2, const angle_unit_t unit)
{
    if (vector1 == 0 || vector2 == 0) {
        return 0.0;
    }

    const double square_module1 = bg_fp64_vector2_get_square_module(vector1);

    if (square_module1 <= BG_FP64_SQUARE_EPSYLON) {
        return 0.0;
    }

    const double square_module2 = bg_fp64_vector2_get_square_module(vector2);

    if (square_module2 <= BG_FP64_SQUARE_EPSYLON) {
        return 0.0;
    }

    const double cosine = bg_fp64_vector2_dot_product(vector1, vector2) / sqrt(square_module1 * square_module2);

    if (cosine >= 1.0 - BG_FP64_EPSYLON) {
        return 0.0;
    }

    if (cosine <= -1.0 + BG_FP64_EPSYLON) {
        return bg_fp64_angle_get_half_circle(unit);
    }

    return bg_fp64_radians_to_units(acos(cosine), unit);
}