101 lines
2.8 KiB
C
101 lines
2.8 KiB
C
#include "vector3.h"
|
|
|
|
// =============== Normalization ================ //
|
|
|
|
int bg_fp32_vector3_normalize(BgFP32Vector3* vector)
|
|
{
|
|
const float square_modulus = bg_fp32_vector3_get_square_modulus(vector);
|
|
|
|
if (1.0f - BG_FP32_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP32_TWO_EPSYLON) {
|
|
return 1;
|
|
}
|
|
|
|
if (square_modulus <= BG_FP32_SQUARE_EPSYLON) {
|
|
bg_fp32_vector3_reset(vector);
|
|
return 0;
|
|
}
|
|
|
|
bg_fp32_vector3_multiply(vector, sqrtf(1.0f / square_modulus), vector);
|
|
return 1;
|
|
}
|
|
|
|
int bg_fp64_vector3_normalize(BgFP64Vector3* vector)
|
|
{
|
|
const double square_modulus = bg_fp64_vector3_get_square_modulus(vector);
|
|
|
|
if (1.0 - BG_FP64_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0 + BG_FP64_TWO_EPSYLON) {
|
|
return 1;
|
|
}
|
|
|
|
if (square_modulus <= BG_FP64_SQUARE_EPSYLON) {
|
|
bg_fp64_vector3_reset(vector);
|
|
return 0;
|
|
}
|
|
|
|
bg_fp64_vector3_multiply(vector, sqrt(1.0 / square_modulus), vector);
|
|
return 1;
|
|
}
|
|
|
|
// =================== Angle ==================== //
|
|
|
|
float bg_fp32_vector3_get_angle(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2, const angle_unit_t unit)
|
|
{
|
|
if (vector1 == 0 || vector2 == 0) {
|
|
return 0.0f;
|
|
}
|
|
|
|
const float square_modulus1 = bg_fp32_vector3_get_square_modulus(vector1);
|
|
|
|
if (square_modulus1 <= BG_FP32_SQUARE_EPSYLON) {
|
|
return 0.0f;
|
|
}
|
|
|
|
const float square_modulus2 = bg_fp32_vector3_get_square_modulus(vector2);
|
|
|
|
if (square_modulus2 <= BG_FP32_SQUARE_EPSYLON) {
|
|
return 0.0f;
|
|
}
|
|
|
|
const float cosine = bg_fp32_vector3_scalar_product(vector1, vector2) / sqrtf(square_modulus1 * square_modulus2);
|
|
|
|
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_vector3_get_angle(const BgFP64Vector3* vector1, const BgFP64Vector3* vector2, const angle_unit_t unit)
|
|
{
|
|
if (vector1 == 0 || vector2 == 0) {
|
|
return 0.0;
|
|
}
|
|
|
|
const double square_modulus1 = bg_fp64_vector3_get_square_modulus(vector1);
|
|
|
|
if (square_modulus1 <= BG_FP64_SQUARE_EPSYLON) {
|
|
return 0.0;
|
|
}
|
|
|
|
const double square_modulus2 = bg_fp64_vector3_get_square_modulus(vector2);
|
|
|
|
if (square_modulus2 <= BG_FP64_SQUARE_EPSYLON) {
|
|
return 0.0;
|
|
}
|
|
|
|
const double cosine = bg_fp64_vector3_scalar_product(vector1, vector2) / sqrt(square_modulus1 * square_modulus2);
|
|
|
|
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);
|
|
}
|