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

Рефакторинг и оптимизация вычислений / Refactoring and optimization of computations

Browse source
This commit is contained in:
Andrey Pokidov 2024-11-26 02:25:04 +07:00
parent 03e390c1d0
commit 2655e43cb4
15 changed files with 810 additions and 829 deletions
Download patch file Download diff file Expand all files Collapse all files

76
basic-geometry/quaternion.c
View file

@ -1,5 +1,53 @@
#include "quaternion.h"
// =============== Normalization ================ //
int bg_fp32_quaternion_normalize(BgFP32Quaternion* quaternion)
{
const float square_modulus = bg_fp32_quaternion_get_square_modulus(quaternion);
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_quaternion_reset(quaternion);
return 0;
}
const float multiplier = sqrtf(1.0f / square_modulus);
quaternion->s0 *= multiplier;
quaternion->x1 *= multiplier;
quaternion->x2 *= multiplier;
quaternion->x3 *= multiplier;
return 1;
}
int bg_fp64_quaternion_normalize(BgFP64Quaternion* quaternion)
{
const double square_modulus = bg_fp64_quaternion_get_square_modulus(quaternion);
if (1.0 - BG_FP64_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0 + BG_FP64_TWO_EPSYLON) {
return 1;
}
if (square_modulus <= BG_FP32_SQUARE_EPSYLON) {
bg_fp64_quaternion_reset(quaternion);
return 0;
}
const double multiplier = sqrt(1.0 / square_modulus);
quaternion->s0 *= multiplier;
quaternion->x1 *= multiplier;
quaternion->x2 *= multiplier;
quaternion->x3 *= multiplier;
return 1;
}
// ============ Make Rotation Matrix ============ //
void bg_fp32_quaternion_get_rotation_matrix(const BgFP32Quaternion* quaternion, BgFP32Matrix3x3* matrix)
@ -155,3 +203,31 @@ void bg_fp64_quaternion_get_reverse_matrix(const BgFP64Quaternion* quaternion, B
matrix->r3c2 = corrector2 * (x2x3 - s0x1);
matrix->r1c3 = corrector2 * (x1x3 - s0x2);
}
// ================== Product =================== //
void bg_fp32_quaternion_get_product(const BgFP32Quaternion* left, const BgFP32Quaternion* right, BgFP32Quaternion* product)
{
const float s0 = (left->s0 * right->s0 - left->x1 * right->x1) - (left->x2 * right->x2 + left->x3 * right->x3);
const float x1 = (left->x1 * right->s0 + left->s0 * right->x1) - (left->x3 * right->x2 - left->x2 * right->x3);
const float x2 = (left->x2 * right->s0 + left->s0 * right->x2) - (left->x1 * right->x3 - left->x3 * right->x1);
const float x3 = (left->x3 * right->s0 + left->s0 * right->x3) - (left->x2 * right->x1 - left->x1 * right->x2);
product->s0 = s0;
product->x1 = x1;
product->x2 = x2;
product->x3 = x3;
}
void bg_fp64_quaternion_get_product(const BgFP64Quaternion* left, const BgFP64Quaternion* right, BgFP64Quaternion* product)
{
const double s0 = (left->s0 * right->s0 - left->x1 * right->x1) - (left->x2 * right->x2 + left->x3 * right->x3);
const double x1 = (left->x1 * right->s0 + left->s0 * right->x1) - (left->x3 * right->x2 - left->x2 * right->x3);
const double x2 = (left->x2 * right->s0 + left->s0 * right->x2) - (left->x1 * right->x3 - left->x3 * right->x1);
const double x3 = (left->x3 * right->s0 + left->s0 * right->x3) - (left->x2 * right->x1 - left->x1 * right->x2);
product->s0 = s0;
product->x1 = x1;
product->x2 = x2;
product->x3 = x3;
}