bgc-c/basic-geometry/vector3.c

#include "vector3.h"

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

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

    const float square_modulus1 = vector3_get_square_modulus_fp32(vector1);

    if (square_modulus1 <= FP32_SQUARE_EPSYLON) {
        return 0.0f;
    }

    const float square_modulus2 = vector3_get_square_modulus_fp32(vector2);

    if (square_modulus2 <= FP32_SQUARE_EPSYLON) {
        return 0.0f;
    }

    const float cosine = vector3_fp32_scalar_product(vector1, vector2) / sqrtf(square_modulus1 * square_modulus2);

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

    if (cosine <= -1.0f + FP32_EPSYLON) {
        return fp32_angle_get_half_circle(unit);
    }

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

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

    const double square_modulus1 = vector3_get_square_modulus_fp64(vector1);

    if (square_modulus1 <= FP64_SQUARE_EPSYLON) {
        return 0.0;
    }

    const double square_modulus2 = vector3_get_square_modulus_fp64(vector2);

    if (square_modulus2 <= FP64_SQUARE_EPSYLON) {
        return 0.0;
    }

    const double cosine = vector3_fp64_scalar_product(vector1, vector2) / sqrt(square_modulus1 * square_modulus2);

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

    if (cosine <= -1.0 + FP64_EPSYLON) {
        return fp64_angle_get_half_circle(unit);
    }

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