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

Реорганизация методов для версоров и тангентов

Browse source
This commit is contained in:
Andrey Pokidov 2025-01-30 01:10:41 +07:00
parent 0027924f86
commit e39765b733
5 changed files with 147 additions and 247 deletions
Download patch file Download diff file Expand all files Collapse all files

232
basic-geometry/versor.h
View file

@ -58,6 +58,10 @@ inline void bgc_versor_reset_fp64(BgcVersorFP64* versor)
// ==================== Set ===================== //
void _bgc_versor_normalize_fp32(const float square_modulus, _BgcDarkTwinVersorFP32* twin);
void _bgc_versor_normalize_fp64(const double square_modulus, _BgcDarkTwinVersorFP64* twin);
inline void bgc_versor_set_values_fp32(const float s0, const float x1, const float x2, const float x3, BgcVersorFP32* versor)
{
_BgcDarkTwinVersorFP32* twin = (_BgcDarkTwinVersorFP32*)versor;
@ -73,20 +77,7 @@ inline void bgc_versor_set_values_fp32(const float s0, const float x1, const flo
return;
}
if (square_modulus <= BGC_SQUARE_EPSYLON_FP32) {
twin->s0 = 1.0f;
twin->x1 = 0.0f;
twin->x2 = 0.0f;
twin->x3 = 0.0f;
return;
}
const float multiplier = sqrtf(1.0f / square_modulus);
twin->s0 *= multiplier;
twin->x1 *= multiplier;
twin->x2 *= multiplier;
twin->x3 *= multiplier;
_bgc_versor_normalize_fp32(square_modulus, twin);
}
inline void bgc_versor_set_values_fp64(const double s0, const double x1, const double x2, const double x3, BgcVersorFP64* versor)
@ -104,20 +95,7 @@ inline void bgc_versor_set_values_fp64(const double s0, const double x1, const d
return;
}
if (square_modulus <= BGC_SQUARE_EPSYLON_FP64) {
twin->s0 = 1.0;
twin->x1 = 0.0;
twin->x2 = 0.0;
twin->x3 = 0.0;
return;
}
const double multiplier = sqrt(1.0 / square_modulus);
twin->s0 *= multiplier;
twin->x1 *= multiplier;
twin->x2 *= multiplier;
twin->x3 *= multiplier;
_bgc_versor_normalize_fp64(square_modulus, twin);
}
// ==================== Copy ==================== //
@ -386,184 +364,58 @@ inline void bgc_versor_set_inverted_fp32_to_fp64(const BgcVersorFP32* versor, Bg
inline void bgc_versor_combine_fp32(const BgcVersorFP32* second, const BgcVersorFP32* first, BgcVersorFP32* result)
{
const float s0 = (second->s0 * first->s0 - second->x1 * first->x1) - (second->x2 * first->x2 + second->x3 * first->x3);
const float x1 = (second->x1 * first->s0 + second->s0 * first->x1) - (second->x3 * first->x2 - second->x2 * first->x3);
const float x2 = (second->x2 * first->s0 + second->s0 * first->x2) - (second->x1 * first->x3 - second->x3 * first->x1);
const float x3 = (second->x3 * first->s0 + second->s0 * first->x3) - (second->x2 * first->x1 - second->x1 * first->x2);
const float square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
_BgcDarkTwinVersorFP32* twin = (_BgcDarkTwinVersorFP32*)result;
twin->s0 = s0;
twin->x1 = x1;
twin->x2 = x2;
twin->x3 = x3;
if (1.0f - BGC_TWO_EPSYLON_FP32 <= square_modulus && square_modulus <= 1.0f + BGC_TWO_EPSYLON_FP32) {
return;
}
const float multiplier = sqrtf(1.0f / square_modulus);
twin->s0 *= multiplier;
twin->x1 *= multiplier;
twin->x2 *= multiplier;
twin->x3 *= multiplier;
bgc_versor_set_values_fp32(
(second->s0 * first->s0 - second->x1 * first->x1) - (second->x2 * first->x2 + second->x3 * first->x3),
(second->x1 * first->s0 + second->s0 * first->x1) - (second->x3 * first->x2 - second->x2 * first->x3),
(second->x2 * first->s0 + second->s0 * first->x2) - (second->x1 * first->x3 - second->x3 * first->x1),
(second->x3 * first->s0 + second->s0 * first->x3) - (second->x2 * first->x1 - second->x1 * first->x2),
result
);
}
inline void bgc_versor_combine_fp64(const BgcVersorFP64* second, const BgcVersorFP64* first, BgcVersorFP64* result)
{
const double s0 = (second->s0 * first->s0 - second->x1 * first->x1) - (second->x2 * first->x2 + second->x3 * first->x3);
const double x1 = (second->x1 * first->s0 + second->s0 * first->x1) - (second->x3 * first->x2 - second->x2 * first->x3);
const double x2 = (second->x2 * first->s0 + second->s0 * first->x2) - (second->x1 * first->x3 - second->x3 * first->x1);
const double x3 = (second->x3 * first->s0 + second->s0 * first->x3) - (second->x2 * first->x1 - second->x1 * first->x2);
const double square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
_BgcDarkTwinVersorFP64* twin = (_BgcDarkTwinVersorFP64*)result;
twin->s0 = s0;
twin->x1 = x1;
twin->x2 = x2;
twin->x3 = x3;
if (1.0 - BGC_TWO_EPSYLON_FP64 <= square_modulus && square_modulus <= 1.0 + BGC_TWO_EPSYLON_FP64) {
return;
}
const double multiplier = sqrt(1.0 / square_modulus);
twin->s0 *= multiplier;
twin->x1 *= multiplier;
twin->x2 *= multiplier;
twin->x3 *= multiplier;
bgc_versor_set_values_fp64(
(second->s0 * first->s0 - second->x1 * first->x1) - (second->x2 * first->x2 + second->x3 * first->x3),
(second->x1 * first->s0 + second->s0 * first->x1) - (second->x3 * first->x2 - second->x2 * first->x3),
(second->x2 * first->s0 + second->s0 * first->x2) - (second->x1 * first->x3 - second->x3 * first->x1),
(second->x3 * first->s0 + second->s0 * first->x3) - (second->x2 * first->x1 - second->x1 * first->x2),
result
);
}
// ============ Combination of three ============ //
inline void bgc_versor_combine3_fp32(const BgcVersorFP32* third, const BgcVersorFP32* second, const BgcVersorFP32* first, BgcVersorFP32* result)
{
const float s0a = (second->s0 * first->s0 - second->x1 * first->x1) - (second->x2 * first->x2 + second->x3 * first->x3);
const float x1a = (second->x1 * first->s0 + second->s0 * first->x1) - (second->x3 * first->x2 - second->x2 * first->x3);
const float x2a = (second->x2 * first->s0 + second->s0 * first->x2) - (second->x1 * first->x3 - second->x3 * first->x1);
const float x3a = (second->x3 * first->s0 + second->s0 * first->x3) - (second->x2 * first->x1 - second->x1 * first->x2);
const float s0 = (second->s0 * first->s0 - second->x1 * first->x1) - (second->x2 * first->x2 + second->x3 * first->x3);
const float x1 = (second->x1 * first->s0 + second->s0 * first->x1) - (second->x3 * first->x2 - second->x2 * first->x3);
const float x2 = (second->x2 * first->s0 + second->s0 * first->x2) - (second->x1 * first->x3 - second->x3 * first->x1);
const float x3 = (second->x3 * first->s0 + second->s0 * first->x3) - (second->x2 * first->x1 - second->x1 * first->x2);
const float s0b = (third->s0 * s0a - third->x1 * x1a) - (third->x2 * x2a + third->x3 * x3a);
const float x1b = (third->x1 * s0a + third->s0 * x1a) - (third->x3 * x2a - third->x2 * x3a);
const float x2b = (third->x2 * s0a + third->s0 * x2a) - (third->x1 * x3a - third->x3 * x1a);
const float x3b = (third->x3 * s0a + third->s0 * x3a) - (third->x2 * x1a - third->x1 * x2a);
const float square_modulus = (s0b * s0b + x1b * x1b) + (x2b * x2b + x3b * x3b);
_BgcDarkTwinVersorFP32* twin = (_BgcDarkTwinVersorFP32*)result;
twin->s0 = s0b;
twin->x1 = x1b;
twin->x2 = x2b;
twin->x3 = x3b;
if (1.0f - BGC_TWO_EPSYLON_FP32 <= square_modulus && square_modulus <= 1.0f + BGC_TWO_EPSYLON_FP32) {
return;
}
const float multiplier = sqrtf(1.0f / square_modulus);
twin->s0 *= multiplier;
twin->x1 *= multiplier;
twin->x2 *= multiplier;
twin->x3 *= multiplier;
bgc_versor_set_values_fp32(
(third->s0 * s0 - third->x1 * x1) - (third->x2 * x2 + third->x3 * x3),
(third->x1 * s0 + third->s0 * x1) - (third->x3 * x2 - third->x2 * x3),
(third->x2 * s0 + third->s0 * x2) - (third->x1 * x3 - third->x3 * x1),
(third->x3 * s0 + third->s0 * x3) - (third->x2 * x1 - third->x1 * x2),
result
);
}
inline void bgc_versor_combine3_fp64(const BgcVersorFP64* third, const BgcVersorFP64* second, const BgcVersorFP64* first, BgcVersorFP64* result)
{
const double s0a = (second->s0 * first->s0 - second->x1 * first->x1) - (second->x2 * first->x2 + second->x3 * first->x3);
const double x1a = (second->x1 * first->s0 + second->s0 * first->x1) - (second->x3 * first->x2 - second->x2 * first->x3);
const double x2a = (second->x2 * first->s0 + second->s0 * first->x2) - (second->x1 * first->x3 - second->x3 * first->x1);
const double x3a = (second->x3 * first->s0 + second->s0 * first->x3) - (second->x2 * first->x1 - second->x1 * first->x2);
const double s0 = (second->s0 * first->s0 - second->x1 * first->x1) - (second->x2 * first->x2 + second->x3 * first->x3);
const double x1 = (second->x1 * first->s0 + second->s0 * first->x1) - (second->x3 * first->x2 - second->x2 * first->x3);
const double x2 = (second->x2 * first->s0 + second->s0 * first->x2) - (second->x1 * first->x3 - second->x3 * first->x1);
const double x3 = (second->x3 * first->s0 + second->s0 * first->x3) - (second->x2 * first->x1 - second->x1 * first->x2);
const double s0b = (third->s0 * s0a - third->x1 * x1a) - (third->x2 * x2a + third->x3 * x3a);
const double x1b = (third->x1 * s0a + third->s0 * x1a) - (third->x3 * x2a - third->x2 * x3a);
const double x2b = (third->x2 * s0a + third->s0 * x2a) - (third->x1 * x3a - third->x3 * x1a);
const double x3b = (third->x3 * s0a + third->s0 * x3a) - (third->x2 * x1a - third->x1 * x2a);
const double square_modulus = (s0b * s0b + x1b * x1b) + (x2b * x2b + x3b * x3b);
_BgcDarkTwinVersorFP64* twin = (_BgcDarkTwinVersorFP64*)result;
twin->s0 = s0b;
twin->x1 = x1b;
twin->x2 = x2b;
twin->x3 = x3b;
if (1.0 - BGC_TWO_EPSYLON_FP64 <= square_modulus && square_modulus <= 1.0 + BGC_TWO_EPSYLON_FP64) {
return;
}
const double multiplier = sqrt(1.0 / square_modulus);
twin->s0 *= multiplier;
twin->x1 *= multiplier;
twin->x2 *= multiplier;
twin->x3 *= multiplier;
}
// ================= Exclusion ================== //
inline void bgc_versor_exclude_fp32(const BgcVersorFP32* basic, const BgcVersorFP32* exclusion, BgcVersorFP32* result)
{
const float s0 = (basic->s0 * exclusion->s0 + basic->x1 * exclusion->x1) + (basic->x2 * exclusion->x2 + basic->x3 * exclusion->x3);
const float x1 = (basic->x1 * exclusion->s0 - basic->s0 * exclusion->x1) + (basic->x3 * exclusion->x2 - basic->x2 * exclusion->x3);
const float x2 = (basic->x2 * exclusion->s0 - basic->s0 * exclusion->x2) + (basic->x1 * exclusion->x3 - basic->x3 * exclusion->x1);
const float x3 = (basic->x3 * exclusion->s0 - basic->s0 * exclusion->x3) + (basic->x2 * exclusion->x1 - basic->x1 * exclusion->x2);
const float square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
_BgcDarkTwinVersorFP32* twin = (_BgcDarkTwinVersorFP32*)result;
twin->s0 = s0;
twin->x1 = x1;
twin->x2 = x2;
twin->x3 = x3;
if (1.0f - BGC_TWO_EPSYLON_FP32 <= square_modulus && square_modulus <= 1.0f + BGC_TWO_EPSYLON_FP32) {
return;
}
const float multiplier = sqrtf(1.0f / square_modulus);
twin->s0 *= multiplier;
twin->x1 *= multiplier;
twin->x2 *= multiplier;
twin->x3 *= multiplier;
}
inline void bgc_versor_exclude_fp64(const BgcVersorFP64* basic, const BgcVersorFP64* exclusion, BgcVersorFP64* result)
{
const double s0 = (basic->s0 * exclusion->s0 + basic->x1 * exclusion->x1) + (basic->x2 * exclusion->x2 + basic->x3 * exclusion->x3);
const double x1 = (basic->x1 * exclusion->s0 - basic->s0 * exclusion->x1) + (basic->x3 * exclusion->x2 - basic->x2 * exclusion->x3);
const double x2 = (basic->x2 * exclusion->s0 - basic->s0 * exclusion->x2) + (basic->x1 * exclusion->x3 - basic->x3 * exclusion->x1);
const double x3 = (basic->x3 * exclusion->s0 - basic->s0 * exclusion->x3) + (basic->x2 * exclusion->x1 - basic->x1 * exclusion->x2);
const double square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
_BgcDarkTwinVersorFP64* twin = (_BgcDarkTwinVersorFP64*)result;
twin->s0 = s0;
twin->x1 = x1;
twin->x2 = x2;
twin->x3 = x3;
if (1.0 - BGC_TWO_EPSYLON_FP64 <= square_modulus && square_modulus <= 1.0 + BGC_TWO_EPSYLON_FP64) {
return;
}
const double multiplier = sqrt(1.0 / square_modulus);
twin->s0 *= multiplier;
twin->x1 *= multiplier;
twin->x2 *= multiplier;
twin->x3 *= multiplier;
bgc_versor_set_values_fp64(
(third->s0 * s0 - third->x1 * x1) - (third->x2 * x2 + third->x3 * x3),
(third->x1 * s0 + third->s0 * x1) - (third->x3 * x2 - third->x2 * x3),
(third->x2 * s0 + third->s0 * x2) - (third->x1 * x3 - third->x3 * x1),
(third->x3 * s0 + third->s0 * x3) - (third->x2 * x1 - third->x1 * x2),
result
);
}
// ================= Rotation3 ================== //