Kimelas/bgc-c
2
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Добавление сферической интерполяции, переход от применения acos к применению atan2, исправление ошибок

Browse source
This commit is contained in:
Andrey Pokidov 2025-03-17 09:56:56 +07:00
parent f06b35ae34
commit 9d7011e81e
17 changed files with 558 additions and 134 deletions
Download patch file Download diff file Expand all files Collapse all files

92
basic-geometry/quaternion.c
View file

@ -1,3 +1,4 @@
#include <math.h>
#include "quaternion.h"
extern inline void bgc_quaternion_reset_fp32(BgcQuaternionFP32* quaternion);
@ -63,8 +64,8 @@ extern inline void bgc_quaternion_multiply_fp64(const BgcQuaternionFP64* multipl
extern inline void bgc_quaternion_divide_fp32(const BgcQuaternionFP32* dividend, const float divisor, BgcQuaternionFP32* quotient);
extern inline void bgc_quaternion_divide_fp64(const BgcQuaternionFP64* dividend, const double divisor, BgcQuaternionFP64* quotient);
extern inline void bgc_quaternion_get_linear_interpolation_fp32(const BgcQuaternionFP32* vector1, const BgcQuaternionFP32* vector2, const float phase, BgcQuaternionFP32* interpolation);
extern inline void bgc_quaternion_get_linear_interpolation_fp64(const BgcQuaternionFP64* vector1, const BgcQuaternionFP64* vector2, const double phase, BgcQuaternionFP64* interpolation);
extern inline void bgc_quaternion_interpolate_linearly_fp32(const BgcQuaternionFP32* vector1, const BgcQuaternionFP32* vector2, const float phase, BgcQuaternionFP32* interpolation);
extern inline void bgc_quaternion_interpolate_linearly_fp64(const BgcQuaternionFP64* vector1, const BgcQuaternionFP64* vector2, const double phase, BgcQuaternionFP64* interpolation);
extern inline int bgc_quaternion_get_rotation_matrix_fp32(const BgcQuaternionFP32* quaternion, BgcMatrix3x3FP32* rotation);
extern inline int bgc_quaternion_get_rotation_matrix_fp64(const BgcQuaternionFP64* quaternion, BgcMatrix3x3FP64* rotation);
@ -75,6 +76,89 @@ extern inline int bgc_quaternion_get_reverse_matrix_fp64(const BgcQuaternionFP64
extern inline int bgc_quaternion_get_both_matrixes_fp32(const BgcQuaternionFP32* quaternion, BgcMatrix3x3FP32* rotation, BgcMatrix3x3FP32* reverse);
extern inline int bgc_quaternion_get_both_matrixes_fp64(const BgcQuaternionFP64* quaternion, BgcMatrix3x3FP64* rotation, BgcMatrix3x3FP64* reverse);
extern inline int bgc_quaternion_are_close_fp32(const BgcQuaternionFP32* quaternion1, const BgcQuaternionFP32* quaternion2);
extern inline int bgc_quaternion_are_close_fp32(const BgcQuaternionFP32* quaternion1, const BgcQuaternionFP32* quaternion2);
extern inline int bgc_quaternion_are_close_fp32(const BgcQuaternionFP32* quaternion1, const BgcQuaternionFP32* quaternion2);
extern inline int bgc_quaternion_are_close_fp32(const BgcQuaternionFP32* quaternion1, const BgcQuaternionFP32* quaternion2);
// =============== Get Exponation =============== //
int bgc_quaternion_get_exponation_fp32(const BgcQuaternionFP32* base, const float exponent, BgcQuaternionFP32* power)
{
const float s0s0 = base->s0 * base->s0;
const float x1x1 = base->x1 * base->x1;
const float x2x2 = base->x2 * base->x2;
const float x3x3 = base->x3 * base->x3;
const float square_vector = x1x1 + (x2x2 + x3x3);
const float square_modulus = (s0s0 + x1x1) + (x2x2 + x3x3);
// square_modulus != square_modulus means checking for NaN value at square_modulus
if (square_modulus != square_modulus) {
return 0;
}
if (square_vector <= BGC_SQUARE_EPSYLON_FP32) {
if (base->s0 < 0.0f) {
return 0;
}
power->s0 = powf(base->s0, exponent);
power->x1 = 0.0f;
power->x2 = 0.0f;
power->x3 = 0.0f;
return 1;
}
const float vector_modulus = sqrtf(square_vector);
const float power_angle = atan2f(vector_modulus, base->s0) * exponent;
const float power_modulus = powf(square_modulus, 0.5f * exponent);
const float multiplier = power_modulus * sinf(power_angle) / vector_modulus;
power->s0 = power_modulus * cosf(power_angle);
power->x1 = base->x1 * multiplier;
power->x2 = base->x2 * multiplier;
power->x3 = base->x3 * multiplier;
return 1;
}
int bgc_quaternion_get_exponation_fp64(const BgcQuaternionFP64* base, const double exponent, BgcQuaternionFP64* power)
{
const double s0s0 = base->s0 * base->s0;
const double x1x1 = base->x1 * base->x1;
const double x2x2 = base->x2 * base->x2;
const double x3x3 = base->x3 * base->x3;
const double square_vector = x1x1 + (x2x2 + x3x3);
const double square_modulus = (s0s0 + x1x1) + (x2x2 + x3x3);
// square_modulus != square_modulus means checking for NaN value at square_modulus
if (square_modulus != square_modulus) {
return 0;
}
if (square_vector <= BGC_SQUARE_EPSYLON_FP64) {
if (base->s0 < 0.0) {
return 0;
}
power->s0 = pow(base->s0, exponent);
power->x1 = 0.0;
power->x2 = 0.0;
power->x3 = 0.0;
return 1;
}
const double vector_modulus = sqrt(square_vector);
const double power_angle = atan2(vector_modulus, base->s0) * exponent;
const double power_modulus = pow(square_modulus, 0.5 * exponent);
const double multiplier = power_modulus * sin(power_angle) / vector_modulus;
power->s0 = power_modulus * cos(power_angle);
power->x1 = base->x1 * multiplier;
power->x2 = base->x2 * multiplier;
power->x3 = base->x3 * multiplier;
return 1;
}