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

14
basic-geometry-dev/main.c
View file

@ -52,7 +52,7 @@ BgFP32Versor * make_random_versors(const unsigned int amount)
void print_versor(const BgFP32Versor* versor) void print_versor(const BgFP32Versor* versor)
{ {
printf("(%f, %f, %f, %f)\n", versor->s0, versor->x1, versor->x2, versor->x3); printf("Versor (%f, %f, %f, %f); Delta = %e\n", versor->s0, versor->x1, versor->x2, versor->x3, bg_fp32_versor_get_modulus(versor) - 1.0f);
} }
void print_vector(const BgFP32Vector3* vector) void print_vector(const BgFP32Vector3* vector)
@ -125,11 +125,11 @@ int main()
const unsigned int amount = 1000000; const unsigned int amount = 1000000;
#ifdef _WIN64 #ifdef _WIN64
ULONGLONG now; ULONGLONG now, start, end;
now = GetTickCount64(); now = GetTickCount64();
srand((unsigned int)(now & 0xfffffff)); srand((unsigned int)(now & 0xfffffff));
#else #else
struct timespec now; struct timespec now, start, end;
clock_gettime(0, &now); clock_gettime(0, &now);
srand((unsigned int)(now.tv_nsec & 0xfffffff)); srand((unsigned int)(now.tv_nsec & 0xfffffff));
#endif // _WIN64 #endif // _WIN64
@ -159,10 +159,14 @@ int main()
} }
#ifdef _WIN64 #ifdef _WIN64
ULONGLONG start, end; end = GetTickCount64();
printf("Setup time: %lld\n", end - now);
start = GetTickCount64(); start = GetTickCount64();
#else #else
struct timespec start, end; clock_gettime(CLOCK_REALTIME, &end);
printf("Time: %lf\n", (end.tv_sec - now.tv_sec) * 1000.0 + (end.tv_nsec - now.tv_nsec) * 0.000001);
clock_gettime(CLOCK_REALTIME, &start); clock_gettime(CLOCK_REALTIME, &start);
#endif // _WIN64 #endif // _WIN64
for (int j = 0; j < 1000; j++) { for (int j = 0; j < 1000; j++) {

104
basic-geometry/matrix2x2.c
View file

@ -1 +1,105 @@
#include "matrix2x2.h" #include "matrix2x2.h"
// ================= Inversion ================== //
int bg_fp32_matrix2x2_invert(BgFP32Matrix2x2* matrix)
{
const float determinant = bg_fp32_matrix2x2_get_determinant(matrix);
if (-BG_FP32_EPSYLON <= determinant && determinant <= BG_FP32_EPSYLON) {
return 0;
}
const float r1c1 = matrix->r2c2;
const float r1c2 = -matrix->r1c2;
const float r2c1 = -matrix->r2c1;
const float r2c2 = matrix->r1c1;
const float multiplier = 1.0f / determinant;
matrix->r1c1 = r1c1 * multiplier;
matrix->r1c2 = r1c2 * multiplier;
matrix->r2c1 = r2c1 * multiplier;
matrix->r2c2 = r2c2 * multiplier;
return 1;
}
int bg_fp64_matrix2x2_invert(BgFP64Matrix2x2* matrix)
{
const double determinant = bg_fp64_matrix2x2_get_determinant(matrix);
if (-BG_FP64_EPSYLON <= determinant && determinant <= BG_FP64_EPSYLON) {
return 0;
}
const double r1c1 = matrix->r2c2;
const double r1c2 = -matrix->r1c2;
const double r2c1 = -matrix->r2c1;
const double r2c2 = matrix->r1c1;
const double multiplier = 1.0 / determinant;
matrix->r1c1 = r1c1 * multiplier;
matrix->r1c2 = r1c2 * multiplier;
matrix->r2c1 = r2c1 * multiplier;
matrix->r2c2 = r2c2 * multiplier;
return 1;
}
// ================ Set Inverted ================ //
int bg_fp32_matrix2x2_set_inverted(const BgFP32Matrix2x2* from, BgFP32Matrix2x2* to)
{
const float determinant = bg_fp32_matrix2x2_get_determinant(from);
if (-BG_FP32_EPSYLON <= determinant && determinant <= BG_FP32_EPSYLON) {
return 0;
}
const float r1c1 = from->r2c2;
const float r1c2 = -from->r1c2;
const float r2c1 = -from->r2c1;
const float r2c2 = from->r1c1;
const float multiplier = 1.0f / determinant;
to->r1c1 = r1c1 * multiplier;
to->r1c2 = r1c2 * multiplier;
to->r2c1 = r2c1 * multiplier;
to->r2c2 = r2c2 * multiplier;
return 1;
}
int bg_fp64_matrix2x2_set_inverted(const BgFP64Matrix2x2* from, BgFP64Matrix2x2* to)
{
const double determinant = bg_fp64_matrix2x2_get_determinant(from);
if (-BG_FP64_EPSYLON <= determinant && determinant <= BG_FP64_EPSYLON) {
return 0;
}
const double r1c1 = from->r2c2;
const double r1c2 = -from->r1c2;
const double r2c1 = -from->r2c1;
const double r2c2 = from->r1c1;
const double multiplier = 1.0 / determinant;
to->r1c1 = r1c1 * multiplier;
to->r1c2 = r1c2 * multiplier;
to->r2c1 = r2c1 * multiplier;
to->r2c2 = r2c2 * multiplier;
return 1;
}

144
basic-geometry/matrix2x2.h
View file

@ -171,51 +171,9 @@ static inline void bg_fp64_matrix2x2_transpose(BgFP64Matrix2x2* matrix)
// ================= Inversion ================== // // ================= Inversion ================== //
static inline int bg_fp32_matrix2x2_invert(BgFP32Matrix2x2* matrix) int bg_fp32_matrix2x2_invert(BgFP32Matrix2x2* matrix);
{
const float determinant = bg_fp32_matrix2x2_get_determinant(matrix);
if (-BG_FP32_EPSYLON <= determinant && determinant <= BG_FP32_EPSYLON) { int bg_fp64_matrix2x2_invert(BgFP64Matrix2x2* matrix);
return 0;
}
const float r1c1 = matrix->r2c2;
const float r1c2 = -matrix->r1c2;
const float r2c1 = -matrix->r2c1;
const float r2c2 = matrix->r1c1;
matrix->r1c1 = r1c1 / determinant;
matrix->r1c2 = r1c2 / determinant;
matrix->r2c1 = r2c1 / determinant;
matrix->r2c2 = r2c2 / determinant;
return 1;
}
static inline int bg_fp64_matrix2x2_invert(BgFP64Matrix2x2* matrix)
{
const double determinant = bg_fp64_matrix2x2_get_determinant(matrix);
if (-BG_FP64_EPSYLON <= determinant && determinant <= BG_FP64_EPSYLON) {
return 0;
}
const double r1c1 = matrix->r2c2;
const double r1c2 = -matrix->r1c2;
const double r2c1 = -matrix->r2c1;
const double r2c2 = matrix->r1c1;
matrix->r1c1 = r1c1 / determinant;
matrix->r1c2 = r1c2 / determinant;
matrix->r2c1 = r2c1 / determinant;
matrix->r2c2 = r2c2 / determinant;
return 1;
}
// =============== Set Transposed =============== // // =============== Set Transposed =============== //
@ -243,51 +201,9 @@ static inline void bg_fp64_matrix2x2_set_transposed(const BgFP64Matrix2x2* from,
// ================ Set Inverted ================ // // ================ Set Inverted ================ //
static inline int bg_fp32_matrix2x2_set_inverted(const BgFP32Matrix2x2* from, BgFP32Matrix2x2* to) int bg_fp32_matrix2x2_set_inverted(const BgFP32Matrix2x2* from, BgFP32Matrix2x2* to);
{
const float determinant = bg_fp32_matrix2x2_get_determinant(from);
if (-BG_FP32_EPSYLON <= determinant && determinant <= BG_FP32_EPSYLON) { int bg_fp64_matrix2x2_set_inverted(const BgFP64Matrix2x2* from, BgFP64Matrix2x2* to);
return 0;
}
const float r1c1 = from->r2c2;
const float r1c2 = -from->r1c2;
const float r2c1 = -from->r2c1;
const float r2c2 = from->r1c1;
to->r1c1 = r1c1 / determinant;
to->r1c2 = r1c2 / determinant;
to->r2c1 = r2c1 / determinant;
to->r2c2 = r2c2 / determinant;
return 1;
}
static inline int bg_fp64_matrix2x2_set_inverted(const BgFP64Matrix2x2* from, BgFP64Matrix2x2* to)
{
const double determinant = bg_fp64_matrix2x2_get_determinant(from);
if (-BG_FP64_EPSYLON <= determinant && determinant <= BG_FP64_EPSYLON) {
return 0;
}
const double r1c1 = from->r2c2;
const double r1c2 = -from->r1c2;
const double r2c1 = -from->r2c1;
const double r2c2 = from->r1c1;
to->r1c1 = r1c1 / determinant;
to->r1c2 = r1c2 / determinant;
to->r2c1 = r2c1 / determinant;
to->r2c2 = r2c2 / determinant;
return 1;
}
// ================= Set Row 1 ================== // // ================= Set Row 1 ================== //
@ -429,60 +345,52 @@ static inline void bg_fp64_matrix2x2_multiply(const BgFP64Matrix2x2* multiplican
static inline void bg_fp32_matrix2x2_divide(const BgFP32Matrix2x2* dividend, const float divisor, BgFP32Matrix2x2* quotient) static inline void bg_fp32_matrix2x2_divide(const BgFP32Matrix2x2* dividend, const float divisor, BgFP32Matrix2x2* quotient)
{ {
quotient->r1c1 = dividend->r1c1 / divisor; bg_fp32_matrix2x2_multiply(dividend, 1.0f / divisor, quotient);
quotient->r1c2 = dividend->r1c2 / divisor;
quotient->r2c1 = dividend->r2c1 / divisor;
quotient->r2c2 = dividend->r2c2 / divisor;
} }
static inline void bg_fp64_matrix2x2_divide(const BgFP64Matrix2x2* dividend, const double divisor, BgFP64Matrix2x2* quotient) static inline void bg_fp64_matrix2x2_divide(const BgFP64Matrix2x2* dividend, const double divisor, BgFP64Matrix2x2* quotient)
{ {
quotient->r1c1 = dividend->r1c1 / divisor; bg_fp64_matrix2x2_multiply(dividend, 1.0 / divisor, quotient);
quotient->r1c2 = dividend->r1c2 / divisor;
quotient->r2c1 = dividend->r2c1 / divisor;
quotient->r2c2 = dividend->r2c2 / divisor;
} }
// ============ Left Vector Product ============= // // ============ Left Vector Product ============= //
static inline void bg_fp32_matrix2x2_left_product(const BgFP32Vector2* vector, const BgFP32Matrix2x2* matrix, BgFP32Vector2* result) static inline void bg_fp32_matrix2x2_left_product(const BgFP32Vector2* vector, const BgFP32Matrix2x2* matrix, BgFP32Vector2* result)
{ {
bg_fp32_vector2_set_values( const float x1 = vector->x1 * matrix->r1c1 + vector->x2 * matrix->r2c1;
vector->x1 * matrix->r1c1 + vector->x2 * matrix->r2c1, const float x2 = vector->x1 * matrix->r1c2 + vector->x2 * matrix->r2c2;
vector->x1 * matrix->r1c2 + vector->x2 * matrix->r2c2,
result result->x1 = x1;
); result->x2 = x2;
} }
static inline void bg_fp64_matrix2x2_left_product(const BgFP64Vector2* vector, const BgFP64Matrix2x2* matrix, BgFP64Vector2* result) static inline void bg_fp64_matrix2x2_left_product(const BgFP64Vector2* vector, const BgFP64Matrix2x2* matrix, BgFP64Vector2* result)
{ {
bg_fp64_vector2_set_values( const double x1 = vector->x1 * matrix->r1c1 + vector->x2 * matrix->r2c1;
vector->x1 * matrix->r1c1 + vector->x2 * matrix->r2c1, const double x2 = vector->x1 * matrix->r1c2 + vector->x2 * matrix->r2c2;
vector->x1 * matrix->r1c2 + vector->x2 * matrix->r2c2,
result result->x1 = x1;
); result->x2 = x2;
} }
// ============ Right Vector Product ============ // // ============ Right Vector Product ============ //
static inline void bg_fp32_matrix2x2_right_product(const BgFP32Matrix2x2* matrix, const BgFP32Vector2* vector, BgFP32Vector2* result) static inline void bg_fp32_matrix2x2_right_product(const BgFP32Matrix2x2* matrix, const BgFP32Vector2* vector, BgFP32Vector2* result)
{ {
bg_fp32_vector2_set_values( const float x1 = matrix->r1c1 * vector->x1 + matrix->r1c2 * vector->x2;
matrix->r1c1 * vector->x1 + matrix->r1c2 * vector->x2, const float x2 = matrix->r2c1 * vector->x1 + matrix->r2c2 * vector->x2;
matrix->r2c1 * vector->x1 + matrix->r2c2 * vector->x2,
result result->x1 = x1;
); result->x2 = x2;
} }
static inline void bg_fp64_matrix2x2_right_product(const BgFP64Matrix2x2* matrix, const BgFP64Vector2* vector, BgFP64Vector2* result) static inline void bg_fp64_matrix2x2_right_product(const BgFP64Matrix2x2* matrix, const BgFP64Vector2* vector, BgFP64Vector2* result)
{ {
bg_fp64_vector2_set_values( const double x1 = matrix->r1c1 * vector->x1 + matrix->r1c2 * vector->x2;
matrix->r1c1 * vector->x1 + matrix->r1c2 * vector->x2, const double x2 = matrix->r2c1 * vector->x1 + matrix->r2c2 * vector->x2;
matrix->r2c1 * vector->x1 + matrix->r2c2 * vector->x2,
result result->x1 = x1;
); result->x2 = x2;
} }
#endif #endif

18
basic-geometry/matrix2x3.h
View file

@ -317,26 +317,12 @@ static inline void bg_fp64_matrix2x3_multiply(const BgFP64Matrix2x3* multiplican
static inline void bg_fp32_matrix2x3_divide(const BgFP32Matrix2x3* dividend, const float divisor, BgFP32Matrix2x3* quotient) static inline void bg_fp32_matrix2x3_divide(const BgFP32Matrix2x3* dividend, const float divisor, BgFP32Matrix2x3* quotient)
{ {
quotient->r1c1 = dividend->r1c1 / divisor; bg_fp32_matrix2x3_multiply(dividend, 1.0f / divisor, quotient);
quotient->r1c2 = dividend->r1c2 / divisor;
quotient->r2c1 = dividend->r2c1 / divisor;
quotient->r2c2 = dividend->r2c2 / divisor;
quotient->r3c1 = dividend->r3c1 / divisor;
quotient->r3c2 = dividend->r3c2 / divisor;
} }
static inline void bg_fp64_matrix2x3_divide(const BgFP64Matrix2x3* dividend, const double divisor, BgFP64Matrix2x3* quotient) static inline void bg_fp64_matrix2x3_divide(const BgFP64Matrix2x3* dividend, const double divisor, BgFP64Matrix2x3* quotient)
{ {
quotient->r1c1 = dividend->r1c1 / divisor; bg_fp64_matrix2x3_multiply(dividend, 1.0 / divisor, quotient);
quotient->r1c2 = dividend->r1c2 / divisor;
quotient->r2c1 = dividend->r2c1 / divisor;
quotient->r2c2 = dividend->r2c2 / divisor;
quotient->r3c1 = dividend->r3c1 / divisor;
quotient->r3c2 = dividend->r3c2 / divisor;
} }
// ============ Left Vector Product ============= // // ============ Left Vector Product ============= //

16
basic-geometry/matrix3x2.h
View file

@ -275,24 +275,12 @@ static inline void bg_fp64_matrix3x2_multiply(const BgFP64Matrix3x2* multiplican
static inline void bg_fp32_matrix3x2_divide(const BgFP32Matrix3x2* dividend, const float divisor, BgFP32Matrix3x2* quotient) static inline void bg_fp32_matrix3x2_divide(const BgFP32Matrix3x2* dividend, const float divisor, BgFP32Matrix3x2* quotient)
{ {
quotient->r1c1 = dividend->r1c1 / divisor; bg_fp32_matrix3x2_multiply(dividend, 1.0f / divisor, quotient);
quotient->r1c2 = dividend->r1c2 / divisor;
quotient->r1c3 = dividend->r1c3 / divisor;
quotient->r2c1 = dividend->r2c1 / divisor;
quotient->r2c2 = dividend->r2c2 / divisor;
quotient->r2c3 = dividend->r2c3 / divisor;
} }
static inline void bg_fp64_matrix3x2_divide(const BgFP64Matrix3x2* dividend, const double divisor, BgFP64Matrix3x2* quotient) static inline void bg_fp64_matrix3x2_divide(const BgFP64Matrix3x2* dividend, const double divisor, BgFP64Matrix3x2* quotient)
{ {
quotient->r1c1 = dividend->r1c1 / divisor; bg_fp64_matrix3x2_multiply(dividend, 1.0 / divisor, quotient);
quotient->r1c2 = dividend->r1c2 / divisor;
quotient->r1c3 = dividend->r1c3 / divisor;
quotient->r2c1 = dividend->r2c1 / divisor;
quotient->r2c2 = dividend->r2c2 / divisor;
quotient->r2c3 = dividend->r2c3 / divisor;
} }
// ============ Left Vector Product ============= // // ============ Left Vector Product ============= //

80
basic-geometry/matrix3x3.c
View file

@ -22,17 +22,19 @@ int bg_fp32_matrix3x3_invert(BgFP32Matrix3x3* matrix)
const float r3c2 = matrix->r1c2 * matrix->r3c1 - matrix->r1c1 * matrix->r3c2; const float r3c2 = matrix->r1c2 * matrix->r3c1 - matrix->r1c1 * matrix->r3c2;
const float r3c3 = matrix->r1c1 * matrix->r2c2 - matrix->r1c2 * matrix->r2c1; const float r3c3 = matrix->r1c1 * matrix->r2c2 - matrix->r1c2 * matrix->r2c1;
matrix->r1c1 = r1c1 / determinant; const float multiplier = 1.0f / determinant;
matrix->r1c2 = r1c2 / determinant;
matrix->r1c3 = r1c3 / determinant;
matrix->r2c1 = r2c1 / determinant; matrix->r1c1 = r1c1 * multiplier;
matrix->r2c2 = r2c2 / determinant; matrix->r1c2 = r1c2 * multiplier;
matrix->r2c3 = r2c3 / determinant; matrix->r1c3 = r1c3 * multiplier;
matrix->r3c1 = r3c1 / determinant; matrix->r2c1 = r2c1 * multiplier;
matrix->r3c2 = r3c2 / determinant; matrix->r2c2 = r2c2 * multiplier;
matrix->r3c3 = r3c3 / determinant; matrix->r2c3 = r2c3 * multiplier;
matrix->r3c1 = r3c1 * multiplier;
matrix->r3c2 = r3c2 * multiplier;
matrix->r3c3 = r3c3 * multiplier;
return 1; return 1;
} }
@ -57,17 +59,19 @@ int bg_fp64_matrix3x3_invert(BgFP64Matrix3x3* matrix)
const double r3c2 = matrix->r1c2 * matrix->r3c1 - matrix->r1c1 * matrix->r3c2; const double r3c2 = matrix->r1c2 * matrix->r3c1 - matrix->r1c1 * matrix->r3c2;
const double r3c3 = matrix->r1c1 * matrix->r2c2 - matrix->r1c2 * matrix->r2c1; const double r3c3 = matrix->r1c1 * matrix->r2c2 - matrix->r1c2 * matrix->r2c1;
matrix->r1c1 = r1c1 / determinant; const double multiplier = 1.0 / determinant;
matrix->r1c2 = r1c2 / determinant;
matrix->r1c3 = r1c3 / determinant;
matrix->r2c1 = r2c1 / determinant; matrix->r1c1 = r1c1 * multiplier;
matrix->r2c2 = r2c2 / determinant; matrix->r1c2 = r1c2 * multiplier;
matrix->r2c3 = r2c3 / determinant; matrix->r1c3 = r1c3 * multiplier;
matrix->r3c1 = r3c1 / determinant; matrix->r2c1 = r2c1 * multiplier;
matrix->r3c2 = r3c2 / determinant; matrix->r2c2 = r2c2 * multiplier;
matrix->r3c3 = r3c3 / determinant; matrix->r2c3 = r2c3 * multiplier;
matrix->r3c1 = r3c1 * multiplier;
matrix->r3c2 = r3c2 * multiplier;
matrix->r3c3 = r3c3 * multiplier;
return 1; return 1;
} }
@ -94,17 +98,19 @@ int bg_fp32_matrix3x3_set_inverted(const BgFP32Matrix3x3* matrix, BgFP32Matrix3x
const float r3c2 = matrix->r1c2 * matrix->r3c1 - matrix->r1c1 * matrix->r3c2; const float r3c2 = matrix->r1c2 * matrix->r3c1 - matrix->r1c1 * matrix->r3c2;
const float r3c3 = matrix->r1c1 * matrix->r2c2 - matrix->r1c2 * matrix->r2c1; const float r3c3 = matrix->r1c1 * matrix->r2c2 - matrix->r1c2 * matrix->r2c1;
result->r1c1 = r1c1 / determinant; const float multiplier = 1.0f / determinant;
result->r1c2 = r1c2 / determinant;
result->r1c3 = r1c3 / determinant;
result->r2c1 = r2c1 / determinant; result->r1c1 = r1c1 * multiplier;
result->r2c2 = r2c2 / determinant; result->r1c2 = r1c2 * multiplier;
result->r2c3 = r2c3 / determinant; result->r1c3 = r1c3 * multiplier;
result->r3c1 = r3c1 / determinant; result->r2c1 = r2c1 * multiplier;
result->r3c2 = r3c2 / determinant; result->r2c2 = r2c2 * multiplier;
result->r3c3 = r3c3 / determinant; result->r2c3 = r2c3 * multiplier;
result->r3c1 = r3c1 * multiplier;
result->r3c2 = r3c2 * multiplier;
result->r3c3 = r3c3 * multiplier;
return 1; return 1;
} }
@ -129,17 +135,19 @@ int bg_fp64_matrix3x3_set_inverted(const BgFP64Matrix3x3* matrix, BgFP64Matrix3x
const double r3c2 = matrix->r1c2 * matrix->r3c1 - matrix->r1c1 * matrix->r3c2; const double r3c2 = matrix->r1c2 * matrix->r3c1 - matrix->r1c1 * matrix->r3c2;
const double r3c3 = matrix->r1c1 * matrix->r2c2 - matrix->r1c2 * matrix->r2c1; const double r3c3 = matrix->r1c1 * matrix->r2c2 - matrix->r1c2 * matrix->r2c1;
result->r1c1 = r1c1 / determinant; const double multiplier = 1.0 / determinant;
result->r1c2 = r1c2 / determinant;
result->r1c3 = r1c3 / determinant;
result->r2c1 = r2c1 / determinant; result->r1c1 = r1c1 * multiplier;
result->r2c2 = r2c2 / determinant; result->r1c2 = r1c2 * multiplier;
result->r2c3 = r2c3 / determinant; result->r1c3 = r1c3 * multiplier;
result->r3c1 = r3c1 / determinant; result->r2c1 = r2c1 * multiplier;
result->r3c2 = r3c2 / determinant; result->r2c2 = r2c2 * multiplier;
result->r3c3 = r3c3 / determinant; result->r2c3 = r2c3 * multiplier;
result->r3c1 = r3c1 * multiplier;
result->r3c2 = r3c2 * multiplier;
result->r3c3 = r3c3 * multiplier;
return 1; return 1;
} }

76
basic-geometry/matrix3x3.h
View file

@ -510,76 +510,60 @@ static inline void bg_fp64_matrix3x3_multiply(const BgFP64Matrix3x3* multiplican
static inline void bg_fp32_matrix3x3_divide(const BgFP32Matrix3x3* dividend, const float divisor, BgFP32Matrix3x3* quotient) static inline void bg_fp32_matrix3x3_divide(const BgFP32Matrix3x3* dividend, const float divisor, BgFP32Matrix3x3* quotient)
{ {
quotient->r1c1 = dividend->r1c1 / divisor; bg_fp32_matrix3x3_multiply(dividend, 1.0f / divisor, quotient);
quotient->r1c2 = dividend->r1c2 / divisor;
quotient->r1c3 = dividend->r1c3 / divisor;
quotient->r2c1 = dividend->r2c1 / divisor;
quotient->r2c2 = dividend->r2c2 / divisor;
quotient->r2c3 = dividend->r2c3 / divisor;
quotient->r3c1 = dividend->r3c1 / divisor;
quotient->r3c2 = dividend->r3c2 / divisor;
quotient->r3c3 = dividend->r3c3 / divisor;
} }
static inline void bg_fp64_matrix3x3_divide(const BgFP64Matrix3x3* dividend, const double divisor, BgFP64Matrix3x3* quotient) static inline void bg_fp64_matrix3x3_divide(const BgFP64Matrix3x3* dividend, const double divisor, BgFP64Matrix3x3* quotient)
{ {
quotient->r1c1 = dividend->r1c1 / divisor; bg_fp64_matrix3x3_multiply(dividend, 1.0 / divisor, quotient);
quotient->r1c2 = dividend->r1c2 / divisor;
quotient->r1c3 = dividend->r1c3 / divisor;
quotient->r2c1 = dividend->r2c1 / divisor;
quotient->r2c2 = dividend->r2c2 / divisor;
quotient->r2c3 = dividend->r2c3 / divisor;
quotient->r3c1 = dividend->r3c1 / divisor;
quotient->r3c2 = dividend->r3c2 / divisor;
quotient->r3c3 = dividend->r3c3 / divisor;
} }
// ============ Left Vector Product ============= // // ============ Left Vector Product ============= //
static inline void bg_fp32_matrix3x3_left_product(const BgFP32Vector3* vector, const BgFP32Matrix3x3* matrix, BgFP32Vector3* result) static inline void bg_fp32_matrix3x3_left_product(const BgFP32Vector3* vector, const BgFP32Matrix3x3* matrix, BgFP32Vector3* result)
{ {
bg_fp32_vector3_set_values( const float x1 = vector->x1 * matrix->r1c1 + vector->x2 * matrix->r2c1 + vector->x3 * matrix->r3c1;
vector->x1 * matrix->r1c1 + vector->x2 * matrix->r2c1 + vector->x3 * matrix->r3c1, const float x2 = vector->x1 * matrix->r1c2 + vector->x2 * matrix->r2c2 + vector->x3 * matrix->r3c2;
vector->x1 * matrix->r1c2 + vector->x2 * matrix->r2c2 + vector->x3 * matrix->r3c2, const float x3 = vector->x1 * matrix->r1c3 + vector->x2 * matrix->r2c3 + vector->x3 * matrix->r3c3;
vector->x1 * matrix->r1c3 + vector->x2 * matrix->r2c3 + vector->x3 * matrix->r3c3,
result result->x1 = x1;
); result->x2 = x2;
result->x3 = x3;
} }
static inline void bg_fp64_matrix3x3_left_product(const BgFP64Vector3* vector, const BgFP64Matrix3x3* matrix, BgFP64Vector3* result) static inline void bg_fp64_matrix3x3_left_product(const BgFP64Vector3* vector, const BgFP64Matrix3x3* matrix, BgFP64Vector3* result)
{ {
bg_fp64_vector3_set_values( const double x1 = vector->x1 * matrix->r1c1 + vector->x2 * matrix->r2c1 + vector->x3 * matrix->r3c1;
vector->x1 * matrix->r1c1 + vector->x2 * matrix->r2c1 + vector->x3 * matrix->r3c1, const double x2 = vector->x1 * matrix->r1c2 + vector->x2 * matrix->r2c2 + vector->x3 * matrix->r3c2;
vector->x1 * matrix->r1c2 + vector->x2 * matrix->r2c2 + vector->x3 * matrix->r3c2, const double x3 = vector->x1 * matrix->r1c3 + vector->x2 * matrix->r2c3 + vector->x3 * matrix->r3c3;
vector->x1 * matrix->r1c3 + vector->x2 * matrix->r2c3 + vector->x3 * matrix->r3c3,
result result->x1 = x1;
); result->x2 = x2;
result->x3 = x3;
} }
// ============ Right Vector Product ============ // // ============ Right Vector Product ============ //
static inline void bg_fp32_matrix3x3_right_product(const BgFP32Matrix3x3* matrix, const BgFP32Vector3* vector, BgFP32Vector3* result) static inline void bg_fp32_matrix3x3_right_product(const BgFP32Matrix3x3* matrix, const BgFP32Vector3* vector, BgFP32Vector3* result)
{ {
bg_fp32_vector3_set_values( const float x1 = matrix->r1c1 * vector->x1 + matrix->r1c2 * vector->x2 + matrix->r1c3 * vector->x3;
matrix->r1c1 * vector->x1 + matrix->r1c2 * vector->x2 + matrix->r1c3 * vector->x3, const float x2 = matrix->r2c1 * vector->x1 + matrix->r2c2 * vector->x2 + matrix->r2c3 * vector->x3;
matrix->r2c1 * vector->x1 + matrix->r2c2 * vector->x2 + matrix->r2c3 * vector->x3, const float x3 = matrix->r3c1 * vector->x1 + matrix->r3c2 * vector->x2 + matrix->r3c3 * vector->x3;
matrix->r3c1 * vector->x1 + matrix->r3c2 * vector->x2 + matrix->r3c3 * vector->x3,
result result->x1 = x1;
); result->x2 = x2;
result->x3 = x3;
} }
static inline void bg_fp64_matrix3x3_right_product(const BgFP64Matrix3x3* matrix, const BgFP64Vector3* vector, BgFP64Vector3* result) static inline void bg_fp64_matrix3x3_right_product(const BgFP64Matrix3x3* matrix, const BgFP64Vector3* vector, BgFP64Vector3* result)
{ {
bg_fp64_vector3_set_values( const double x1 = matrix->r1c1 * vector->x1 + matrix->r1c2 * vector->x2 + matrix->r1c3 * vector->x3;
matrix->r1c1 * vector->x1 + matrix->r1c2 * vector->x2 + matrix->r1c3 * vector->x3, const double x2 = matrix->r2c1 * vector->x1 + matrix->r2c2 * vector->x2 + matrix->r2c3 * vector->x3;
matrix->r2c1 * vector->x1 + matrix->r2c2 * vector->x2 + matrix->r2c3 * vector->x3, const double x3 = matrix->r3c1 * vector->x1 + matrix->r3c2 * vector->x2 + matrix->r3c3 * vector->x3;
matrix->r3c1 * vector->x1 + matrix->r3c2 * vector->x2 + matrix->r3c3 * vector->x3,
result result->x1 = x1;
); result->x2 = x2;
result->x3 = x3;
} }
#endif #endif

76
basic-geometry/quaternion.c
View file

@ -1,5 +1,53 @@
#include "quaternion.h" #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 ============ // // ============ Make Rotation Matrix ============ //
void bg_fp32_quaternion_get_rotation_matrix(const BgFP32Quaternion* quaternion, BgFP32Matrix3x3* 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->r3c2 = corrector2 * (x2x3 - s0x1);
matrix->r1c3 = corrector2 * (x1x3 - s0x2); 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;
}

74
basic-geometry/quaternion.h
View file

@ -181,6 +181,12 @@ static inline double bg_fp64_quaternion_get_modulus(const BgFP64Quaternion* quat
return sqrt(bg_fp64_quaternion_get_square_modulus(quaternion)); return sqrt(bg_fp64_quaternion_get_square_modulus(quaternion));
} }
// =============== Normalization ================ //
int bg_fp32_quaternion_normalize(BgFP32Quaternion* quaternion);
int bg_fp64_quaternion_normalize(BgFP64Quaternion* quaternion);
// ============ Make Rotation Matrix ============ // // ============ Make Rotation Matrix ============ //
void bg_fp32_quaternion_get_rotation_matrix(const BgFP32Quaternion* quaternion, BgFP32Matrix3x3* matrix); void bg_fp32_quaternion_get_rotation_matrix(const BgFP32Quaternion* quaternion, BgFP32Matrix3x3* matrix);
@ -213,48 +219,56 @@ static inline void bg_fp64_quaternion_add(const BgFP64Quaternion * quaternion1,
// ================== Subtract ================== // // ================== Subtract ================== //
static inline void bg_fp32_quaternion_subtract(const BgFP32Quaternion * minuend, const BgFP32Quaternion * subtrahend, BgFP32Quaternion * result) static inline void bg_fp32_quaternion_subtract(const BgFP32Quaternion * minuend, const BgFP32Quaternion * subtrahend, BgFP32Quaternion * difference)
{ {
result->s0 = minuend->s0 - subtrahend->s0; difference->s0 = minuend->s0 - subtrahend->s0;
result->x1 = minuend->x1 - subtrahend->x1; difference->x1 = minuend->x1 - subtrahend->x1;
result->x2 = minuend->x2 - subtrahend->x2; difference->x2 = minuend->x2 - subtrahend->x2;
result->x3 = minuend->x3 - subtrahend->x3; difference->x3 = minuend->x3 - subtrahend->x3;
} }
static inline void bg_fp64_quaternion_subtract(const BgFP64Quaternion * minuend, const BgFP64Quaternion * subtrahend, BgFP64Quaternion * result) static inline void bg_fp64_quaternion_subtract(const BgFP64Quaternion * minuend, const BgFP64Quaternion * subtrahend, BgFP64Quaternion * difference)
{ {
result->s0 = minuend->s0 - subtrahend->s0; difference->s0 = minuend->s0 - subtrahend->s0;
result->x1 = minuend->x1 - subtrahend->x1; difference->x1 = minuend->x1 - subtrahend->x1;
result->x2 = minuend->x2 - subtrahend->x2; difference->x2 = minuend->x2 - subtrahend->x2;
result->x3 = minuend->x3 - subtrahend->x3; difference->x3 = minuend->x3 - subtrahend->x3;
} }
// =============== Multiplication =============== // // =============== Multiplication =============== //
static inline void bg_fp32_quaternion_multiply(const BgFP32Quaternion* left, const BgFP32Quaternion* right, BgFP32Quaternion* result) static inline void bg_fp32_quaternion_multiply(const BgFP32Quaternion* multiplicand, const float multipier, BgFP32Quaternion* product)
{ {
const float s0 = (left->s0 * right->s0 - left->x1 * right->x1) - (left->x2 * right->x2 + left->x3 * right->x3); product->s0 = multiplicand->s0 * multipier;
const float x1 = (left->x1 * right->s0 + left->s0 * right->x1) - (left->x3 * right->x2 - left->x2 * right->x3); product->x1 = multiplicand->x1 * multipier;
const float x2 = (left->x2 * right->s0 + left->s0 * right->x2) - (left->x1 * right->x3 - left->x3 * right->x1); product->x2 = multiplicand->x2 * multipier;
const float x3 = (left->x3 * right->s0 + left->s0 * right->x3) - (left->x2 * right->x1 - left->x1 * right->x2); product->x3 = multiplicand->x3 * multipier;
result->s0 = s0;
result->x1 = x1;
result->x2 = x2;
result->x3 = x3;
} }
static inline void bg_fp64_quaternion_multiply(const BgFP64Quaternion* left, const BgFP64Quaternion* right, BgFP64Quaternion* result) static inline void bg_fp64_quaternion_multiply(const BgFP64Quaternion* multiplicand, const double multipier, BgFP64Quaternion* product)
{ {
const double s0 = (left->s0 * right->s0 - left->x1 * right->x1) - (left->x2 * right->x2 + left->x3 * right->x3); product->s0 = multiplicand->s0 * multipier;
const double x1 = (left->x1 * right->s0 + left->s0 * right->x1) - (left->x3 * right->x2 - left->x2 * right->x3); product->x1 = multiplicand->x1 * multipier;
const double x2 = (left->x2 * right->s0 + left->s0 * right->x2) - (left->x1 * right->x3 - left->x3 * right->x1); product->x2 = multiplicand->x2 * multipier;
const double x3 = (left->x3 * right->s0 + left->s0 * right->x3) - (left->x2 * right->x1 - left->x1 * right->x2); product->x3 = multiplicand->x3 * multipier;
result->s0 = s0;
result->x1 = x1;
result->x2 = x2;
result->x3 = x3;
} }
// ================== Division ================== //
static inline void bg_fp32_quaternion_divide(const BgFP32Quaternion* dividend, const float divisor, BgFP32Quaternion* quotient)
{
bg_fp32_quaternion_multiply(dividend, 1.0f / divisor, quotient);
}
static inline void bg_fp64_quaternion_divide(const BgFP64Quaternion* dividend, const double divisor, BgFP64Quaternion* quotient)
{
bg_fp64_quaternion_multiply(dividend, 1.0 / divisor, quotient);
}
// ================== Product =================== //
void bg_fp32_quaternion_get_product(const BgFP32Quaternion* left, const BgFP32Quaternion* right, BgFP32Quaternion* product);
void bg_fp64_quaternion_get_product(const BgFP64Quaternion* left, const BgFP64Quaternion* right, BgFP64Quaternion* product);
#endif // _GEOMETRY_QUATERNION_H_ #endif // _GEOMETRY_QUATERNION_H_

40
basic-geometry/vector2.c
View file

@ -1,5 +1,41 @@
#include "vector2.h" #include "vector2.h"
// =============== Normalization ================ //
int bg_fp32_vector2_normalize(BgFP32Vector2* vector)
{
const float square_modulus = bg_fp32_vector2_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_vector2_reset(vector);
return 0;
}
bg_fp32_vector2_multiply(vector, sqrtf(1.0f / square_modulus), vector);
return 1;
}
int bg_fp64_vector2_normalize(BgFP64Vector2* vector)
{
const double square_modulus = bg_fp64_vector2_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_vector2_reset(vector);
return 0;
}
bg_fp64_vector2_multiply(vector, sqrt(1.0 / square_modulus), vector);
return 1;
}
// =================== Angle ==================== // // =================== Angle ==================== //
float bg_fp32_vector2_get_angle(const BgFP32Vector2* vector1, const BgFP32Vector2* vector2, const angle_unit_t unit) float bg_fp32_vector2_get_angle(const BgFP32Vector2* vector1, const BgFP32Vector2* vector2, const angle_unit_t unit)
@ -20,7 +56,7 @@ float bg_fp32_vector2_get_angle(const BgFP32Vector2* vector1, const BgFP32Vector
return 0.0f; return 0.0f;
} }
const float cosine = bg_fp32_vector2_dot_product(vector1, vector2) / sqrtf(square_modulus1 * square_modulus2); const float cosine = bg_fp32_vector2_scalar_product(vector1, vector2) / sqrtf(square_modulus1 * square_modulus2);
if (cosine >= 1.0f - BG_FP32_EPSYLON) { if (cosine >= 1.0f - BG_FP32_EPSYLON) {
return 0.0f; return 0.0f;
@ -51,7 +87,7 @@ double bg_fp64_vector2_get_angle(const BgFP64Vector2* vector1, const BgFP64Vecto
return 0.0; return 0.0;
} }
const double cosine = bg_fp64_vector2_dot_product(vector1, vector2) / sqrt(square_modulus1 * square_modulus2); const double cosine = bg_fp64_vector2_scalar_product(vector1, vector2) / sqrt(square_modulus1 * square_modulus2);
if (cosine >= 1.0 - BG_FP64_EPSYLON) { if (cosine >= 1.0 - BG_FP64_EPSYLON) {
return 0.0; return 0.0;

84
basic-geometry/vector2.h
View file

@ -150,58 +150,56 @@ static inline int bg_fp64_vector2_is_unit(const BgFP64Vector2* vector)
// ==================== Add ===================== // // ==================== Add ===================== //
static inline void bg_fp32_vector2_add(const BgFP32Vector2* vector1, const BgFP32Vector2* vector2, BgFP32Vector2* result) static inline void bg_fp32_vector2_add(const BgFP32Vector2* vector1, const BgFP32Vector2* vector2, BgFP32Vector2* sum)
{ {
result->x1 = vector1->x1 + vector2->x1; sum->x1 = vector1->x1 + vector2->x1;
result->x2 = vector1->x2 + vector2->x2; sum->x2 = vector1->x2 + vector2->x2;
} }
static inline void bg_fp64_vector2_add(const BgFP64Vector2* vector1, const BgFP64Vector2* vector2, BgFP64Vector2* result) static inline void bg_fp64_vector2_add(const BgFP64Vector2* vector1, const BgFP64Vector2* vector2, BgFP64Vector2* sum)
{ {
result->x1 = vector1->x1 + vector2->x1; sum->x1 = vector1->x1 + vector2->x1;
result->x2 = vector1->x2 + vector2->x2; sum->x2 = vector1->x2 + vector2->x2;
} }
// ================ Subtraction ================= // // ================ Subtraction ================= //
static inline void bg_fp32_vector2_subtract(const BgFP32Vector2* minuend, const BgFP32Vector2* subtrahend, BgFP32Vector2* result) static inline void bg_fp32_vector2_subtract(const BgFP32Vector2* minuend, const BgFP32Vector2* subtrahend, BgFP32Vector2* difference)
{ {
result->x1 = minuend->x1 - subtrahend->x1; difference->x1 = minuend->x1 - subtrahend->x1;
result->x2 = minuend->x2 - subtrahend->x2; difference->x2 = minuend->x2 - subtrahend->x2;
} }
static inline void bg_fp64_vector2_subtract(const BgFP64Vector2* minuend, const BgFP64Vector2* subtrahend, BgFP64Vector2* result) static inline void bg_fp64_vector2_subtract(const BgFP64Vector2* minuend, const BgFP64Vector2* subtrahend, BgFP64Vector2* difference)
{ {
result->x1 = minuend->x1 - subtrahend->x1; difference->x1 = minuend->x1 - subtrahend->x1;
result->x2 = minuend->x2 - subtrahend->x2; difference->x2 = minuend->x2 - subtrahend->x2;
} }
// =============== Multiplication =============== // // =============== Multiplication =============== //
static inline void bg_fp32_vector2_multiply(const BgFP32Vector2* multiplicand, const float multiplier, BgFP32Vector2* result) static inline void bg_fp32_vector2_multiply(const BgFP32Vector2* multiplicand, const float multiplier, BgFP32Vector2* product)
{ {
result->x1 = multiplicand->x1 * multiplier; product->x1 = multiplicand->x1 * multiplier;
result->x2 = multiplicand->x2 * multiplier; product->x2 = multiplicand->x2 * multiplier;
} }
static inline void bg_fp64_vector2_multiply(const BgFP64Vector2* multiplicand, const double multiplier, BgFP64Vector2* result) static inline void bg_fp64_vector2_multiply(const BgFP64Vector2* multiplicand, const double multiplier, BgFP64Vector2* product)
{ {
result->x1 = multiplicand->x1 * multiplier; product->x1 = multiplicand->x1 * multiplier;
result->x2 = multiplicand->x2 * multiplier; product->x2 = multiplicand->x2 * multiplier;
} }
// ================== Division ================== // // ================== Division ================== //
static inline void bg_fp32_vector2_divide(const BgFP32Vector2* dividend, const float divisor, BgFP32Vector2* result) static inline void bg_fp32_vector2_divide(const BgFP32Vector2* dividend, const float divisor, BgFP32Vector2* quotient)
{ {
result->x1 = dividend->x1 / divisor; bg_fp32_vector2_multiply(dividend, 1.0f / divisor, quotient);
result->x2 = dividend->x2 / divisor;
} }
static inline void bg_fp64_vector2_divide(const BgFP64Vector2* dividend, const double divisor, BgFP64Vector2* result) static inline void bg_fp64_vector2_divide(const BgFP64Vector2* dividend, const double divisor, BgFP64Vector2* quotient)
{ {
result->x1 = dividend->x1 / divisor; bg_fp64_vector2_multiply(dividend, 1.0 / divisor, quotient);
result->x2 = dividend->x2 / divisor;
} }
// ================ Append scaled =============== // // ================ Append scaled =============== //
@ -248,12 +246,12 @@ static inline void bg_fp64_vector2_get_mean3(const BgFP64Vector2* vector1, const
// =============== Scalar Product =============== // // =============== Scalar Product =============== //
static inline float bg_fp32_vector2_dot_product(const BgFP32Vector2* vector1, const BgFP32Vector2* vector2) static inline float bg_fp32_vector2_scalar_product(const BgFP32Vector2* vector1, const BgFP32Vector2* vector2)
{ {
return vector1->x1 * vector2->x1 + vector1->x2 * vector2->x2; return vector1->x1 * vector2->x1 + vector1->x2 * vector2->x2;
} }
static inline double bg_fp64_vector2_dot_product(const BgFP64Vector2* vector1, const BgFP64Vector2* vector2) static inline double bg_fp64_vector2_scalar_product(const BgFP64Vector2* vector1, const BgFP64Vector2* vector2)
{ {
return vector1->x1 * vector2->x1 + vector1->x2 * vector2->x2; return vector1->x1 * vector2->x1 + vector1->x2 * vector2->x2;
} }
@ -272,39 +270,9 @@ static inline double bg_fp64_vector2_cross_product(const BgFP64Vector2* vector1,
// =============== Normalization ================ // // =============== Normalization ================ //
static inline int bg_fp32_vector2_normalize(BgFP32Vector2* vector) int bg_fp32_vector2_normalize(BgFP32Vector2* vector);
{
const float square_modulus = bg_fp32_vector2_get_square_modulus(vector);
if (1.0f - BG_FP32_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP32_TWO_EPSYLON) { int bg_fp64_vector2_normalize(BgFP64Vector2* vector);
return 1;
}
if (square_modulus <= BG_FP32_SQUARE_EPSYLON) {
bg_fp32_vector2_reset(vector);
return 0;
}
bg_fp32_vector2_divide(vector, sqrtf(square_modulus), vector);
return 1;
}
static inline int bg_fp64_vector2_normalize(BgFP64Vector2* vector)
{
const double square_modulus = bg_fp64_vector2_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_vector2_reset(vector);
return 0;
}
bg_fp64_vector2_divide(vector, sqrt(square_modulus), vector);
return 1;
}
// =============== Get Normalized =============== // // =============== Get Normalized =============== //

40
basic-geometry/vector3.c
View file

@ -1,5 +1,41 @@
#include "vector3.h" #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 ==================== // // =================== Angle ==================== //
float bg_fp32_vector3_get_angle(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2, const angle_unit_t unit) float bg_fp32_vector3_get_angle(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2, const angle_unit_t unit)
@ -20,7 +56,7 @@ float bg_fp32_vector3_get_angle(const BgFP32Vector3* vector1, const BgFP32Vector
return 0.0f; return 0.0f;
} }
const float cosine = bg_fp32_vector3_dot_product(vector1, vector2) / sqrtf(square_modulus1 * square_modulus2); const float cosine = bg_fp32_vector3_scalar_product(vector1, vector2) / sqrtf(square_modulus1 * square_modulus2);
if (cosine >= 1.0f - BG_FP32_EPSYLON) { if (cosine >= 1.0f - BG_FP32_EPSYLON) {
return 0.0f; return 0.0f;
@ -51,7 +87,7 @@ double bg_fp64_vector3_get_angle(const BgFP64Vector3* vector1, const BgFP64Vecto
return 0.0; return 0.0;
} }
const double cosine = bg_fp64_vector3_dot_product(vector1, vector2) / sqrt(square_modulus1 * square_modulus2); const double cosine = bg_fp64_vector3_scalar_product(vector1, vector2) / sqrt(square_modulus1 * square_modulus2);
if (cosine >= 1.0 - BG_FP64_EPSYLON) { if (cosine >= 1.0 - BG_FP64_EPSYLON) {
return 0.0; return 0.0;

132
basic-geometry/vector3.h
View file

@ -164,66 +164,62 @@ static inline int bg_fp64_vector3_is_unit(const BgFP64Vector3* vector)
// ==================== Add ===================== // // ==================== Add ===================== //
static inline void bg_fp32_vector3_add(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2, BgFP32Vector3* result) static inline void bg_fp32_vector3_add(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2, BgFP32Vector3* sum)
{ {
result->x1 = vector1->x1 + vector2->x1; sum->x1 = vector1->x1 + vector2->x1;
result->x2 = vector1->x2 + vector2->x2; sum->x2 = vector1->x2 + vector2->x2;
result->x3 = vector1->x3 + vector2->x3; sum->x3 = vector1->x3 + vector2->x3;
} }
static inline void bg_fp64_vector3_add(const BgFP64Vector3* vector1, const BgFP64Vector3* vector2, BgFP64Vector3* result) static inline void bg_fp64_vector3_add(const BgFP64Vector3* vector1, const BgFP64Vector3* vector2, BgFP64Vector3* sum)
{ {
result->x1 = vector1->x1 + vector2->x1; sum->x1 = vector1->x1 + vector2->x1;
result->x2 = vector1->x2 + vector2->x2; sum->x2 = vector1->x2 + vector2->x2;
result->x3 = vector1->x3 + vector2->x3; sum->x3 = vector1->x3 + vector2->x3;
} }
// ================ Subtraction ================= // // ================ Subtraction ================= //
static inline void bg_fp32_vector3_subtract(const BgFP32Vector3* minuend, const BgFP32Vector3* subtrahend, BgFP32Vector3* result) static inline void bg_fp32_vector3_subtract(const BgFP32Vector3* minuend, const BgFP32Vector3* subtrahend, BgFP32Vector3* difference)
{ {
result->x1 = minuend->x1 - subtrahend->x1; difference->x1 = minuend->x1 - subtrahend->x1;
result->x2 = minuend->x2 - subtrahend->x2; difference->x2 = minuend->x2 - subtrahend->x2;
result->x3 = minuend->x3 - subtrahend->x3; difference->x3 = minuend->x3 - subtrahend->x3;
} }
static inline void bg_fp64_vector3_subtract(const BgFP64Vector3* minuend, const BgFP64Vector3* subtrahend, BgFP64Vector3* result) static inline void bg_fp64_vector3_subtract(const BgFP64Vector3* minuend, const BgFP64Vector3* subtrahend, BgFP64Vector3* difference)
{ {
result->x1 = minuend->x1 - subtrahend->x1; difference->x1 = minuend->x1 - subtrahend->x1;
result->x2 = minuend->x2 - subtrahend->x2; difference->x2 = minuend->x2 - subtrahend->x2;
result->x3 = minuend->x3 - subtrahend->x3; difference->x3 = minuend->x3 - subtrahend->x3;
} }
// =============== Multiplication =============== // // =============== Multiplication =============== //
static inline void bg_fp32_vector3_multiply(const BgFP32Vector3* multiplicand, const float multiplier, BgFP32Vector3* result) static inline void bg_fp32_vector3_multiply(const BgFP32Vector3* multiplicand, const float multiplier, BgFP32Vector3* product)
{ {
result->x1 = multiplicand->x1 * multiplier; product->x1 = multiplicand->x1 * multiplier;
result->x2 = multiplicand->x2 * multiplier; product->x2 = multiplicand->x2 * multiplier;
result->x3 = multiplicand->x3 * multiplier; product->x3 = multiplicand->x3 * multiplier;
} }
static inline void bg_fp64_vector3_multiply(const BgFP64Vector3* multiplicand, const double multiplier, BgFP64Vector3* result) static inline void bg_fp64_vector3_multiply(const BgFP64Vector3* multiplicand, const double multiplier, BgFP64Vector3* product)
{ {
result->x1 = multiplicand->x1 * multiplier; product->x1 = multiplicand->x1 * multiplier;
result->x2 = multiplicand->x2 * multiplier; product->x2 = multiplicand->x2 * multiplier;
result->x3 = multiplicand->x3 * multiplier; product->x3 = multiplicand->x3 * multiplier;
} }
// ================== Division ================== // // ================== Division ================== //
static inline void bg_fp32_vector3_divide(const BgFP32Vector3* dividend, const float divisor, BgFP32Vector3* result) static inline void bg_fp32_vector3_divide(const BgFP32Vector3* dividend, const float divisor, BgFP32Vector3* quotient)
{ {
result->x1 = dividend->x1 / divisor; bg_fp32_vector3_multiply(dividend, 1.0f / divisor, quotient);
result->x2 = dividend->x2 / divisor;
result->x3 = dividend->x3 / divisor;
} }
static inline void bg_fp64_vector3_divide(const BgFP64Vector3* dividend, const double divisor, BgFP64Vector3* result) static inline void bg_fp64_vector3_divide(const BgFP64Vector3* dividend, const double divisor, BgFP64Vector3* quotient)
{ {
result->x1 = dividend->x1 / divisor; bg_fp64_vector3_multiply(dividend, 1.0 / divisor, quotient);
result->x2 = dividend->x2 / divisor;
result->x3 = dividend->x3 / divisor;
} }
// ================ Append scaled =============== // // ================ Append scaled =============== //
@ -276,12 +272,12 @@ static inline void bg_fp64_vector3_get_mean3(const BgFP64Vector3* vector1, const
// =============== Scalar Product =============== // // =============== Scalar Product =============== //
static inline float bg_fp32_vector3_dot_product(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2) static inline float bg_fp32_vector3_scalar_product(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2)
{ {
return vector1->x1 * vector2->x1 + vector1->x2 * vector2->x2 + vector1->x3 * vector2->x3; return vector1->x1 * vector2->x1 + vector1->x2 * vector2->x2 + vector1->x3 * vector2->x3;
} }
static inline double bg_fp64_vector3_dot_product(const BgFP64Vector3* vector1, const BgFP64Vector3* vector2) static inline double bg_fp64_vector3_scalar_product(const BgFP64Vector3* vector1, const BgFP64Vector3* vector2)
{ {
return vector1->x1 * vector2->x1 + vector1->x2 * vector2->x2 + vector1->x3 * vector2->x3; return vector1->x1 * vector2->x1 + vector1->x2 * vector2->x2 + vector1->x3 * vector2->x3;
} }
@ -306,30 +302,32 @@ static inline double bg_fp64_vector3_triple_product(const BgFP64Vector3* vector1
static inline void bg_fp32_vector3_cross_product(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2, BgFP32Vector3* result) static inline void bg_fp32_vector3_cross_product(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2, BgFP32Vector3* result)
{ {
bg_fp32_vector3_set_values( const float x1 = vector1->x2 * vector2->x3 - vector1->x3 * vector2->x2;
vector1->x2 * vector2->x3 - vector1->x3 * vector2->x2, const float x2 = vector1->x3 * vector2->x1 - vector1->x1 * vector2->x3;
vector1->x3 * vector2->x1 - vector1->x1 * vector2->x3, const float x3 = vector1->x1 * vector2->x2 - vector1->x2 * vector2->x1;
vector1->x1 * vector2->x2 - vector1->x2 * vector2->x1,
result result->x1 = x1;
); result->x2 = x2;
result->x3 = x3;
} }
static inline void bg_fp64_vector3_cross_product(const BgFP64Vector3* vector1, const BgFP64Vector3* vector2, BgFP64Vector3* result) static inline void bg_fp64_vector3_cross_product(const BgFP64Vector3* vector1, const BgFP64Vector3* vector2, BgFP64Vector3* result)
{ {
bg_fp64_vector3_set_values( const double x1 = vector1->x2 * vector2->x3 - vector1->x3 * vector2->x2;
vector1->x2 * vector2->x3 - vector1->x3 * vector2->x2, const double x2 = vector1->x3 * vector2->x1 - vector1->x1 * vector2->x3;
vector1->x3 * vector2->x1 - vector1->x1 * vector2->x3, const double x3 = vector1->x1 * vector2->x2 - vector1->x2 * vector2->x1;
vector1->x1 * vector2->x2 - vector1->x2 * vector2->x1,
result result->x1 = x1;
); result->x2 = x2;
result->x3 = x3;
} }
// ============ Double Cross Product ============ // // ============ Double Cross Product ============ //
static inline void bg_fp32_vector3_double_cross_product(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2, const BgFP32Vector3* vector3, BgFP32Vector3* result) static inline void bg_fp32_vector3_double_cross_product(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2, const BgFP32Vector3* vector3, BgFP32Vector3* result)
{ {
const float ac = bg_fp32_vector3_dot_product(vector1, vector3); const float ac = bg_fp32_vector3_scalar_product(vector1, vector3);
const float ab = bg_fp32_vector3_dot_product(vector1, vector2); const float ab = bg_fp32_vector3_scalar_product(vector1, vector2);
result->x1 = vector2->x1 * ac - vector3->x1 * ab; result->x1 = vector2->x1 * ac - vector3->x1 * ab;
result->x2 = vector2->x2 * ac - vector3->x2 * ab; result->x2 = vector2->x2 * ac - vector3->x2 * ab;
@ -338,8 +336,8 @@ static inline void bg_fp32_vector3_double_cross_product(const BgFP32Vector3* vec
static inline void bg_fp64_vector3_double_cross(const BgFP64Vector3* vector1, const BgFP64Vector3* vector2, const BgFP64Vector3* vector3, BgFP64Vector3* result) static inline void bg_fp64_vector3_double_cross(const BgFP64Vector3* vector1, const BgFP64Vector3* vector2, const BgFP64Vector3* vector3, BgFP64Vector3* result)
{ {
const double ac = bg_fp64_vector3_dot_product(vector1, vector3); const double ac = bg_fp64_vector3_scalar_product(vector1, vector3);
const double ab = bg_fp64_vector3_dot_product(vector1, vector2); const double ab = bg_fp64_vector3_scalar_product(vector1, vector2);
result->x1 = vector2->x1 * ac - vector3->x1 * ab; result->x1 = vector2->x1 * ac - vector3->x1 * ab;
result->x2 = vector2->x2 * ac - vector3->x2 * ab; result->x2 = vector2->x2 * ac - vector3->x2 * ab;
@ -348,39 +346,9 @@ static inline void bg_fp64_vector3_double_cross(const BgFP64Vector3* vector1, co
// =============== Normalization ================ // // =============== Normalization ================ //
static inline int bg_fp32_vector3_normalize(BgFP32Vector3* vector) 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) { int bg_fp64_vector3_normalize(BgFP64Vector3* vector);
return 1;
}
if (square_modulus <= BG_FP32_SQUARE_EPSYLON) {
bg_fp32_vector3_reset(vector);
return 0;
}
bg_fp32_vector3_divide(vector, sqrtf(square_modulus), vector);
return 1;
}
static inline 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_divide(vector, sqrt(square_modulus), vector);
return 1;
}
// =============== Get Normalized =============== // // =============== Get Normalized =============== //

327
basic-geometry/versor.c
View file

@ -7,9 +7,24 @@ const BgFP32Versor BG_FP32_IDLE_VERSOR = { 1.0f, 0.0f, 0.0f, 0.0f };
const BgFP64Versor BG_FP64_IDLE_VERSOR = { 1.0, 0.0, 0.0, 0.0 }; const BgFP64Versor BG_FP64_IDLE_VERSOR = { 1.0, 0.0, 0.0, 0.0 };
void __bg_fp32_versor_normalize(const float square_modulus, __BgFP32DarkTwinVersor* twin) // ==================== Set ===================== //
void bg_fp32_versor_set_values(const float s0, const float x1, const float x2, const float x3, BgFP32Versor* versor)
{ {
if (square_modulus <= BG_FP32_SQUARE_EPSYLON || (twin->s0 * twin->s0) >= (1.0f - BG_FP32_TWO_EPSYLON) * square_modulus) { __BgFP32DarkTwinVersor* twin = (__BgFP32DarkTwinVersor*)versor;
twin->s0 = s0;
twin->x1 = x1;
twin->x2 = x2;
twin->x3 = x3;
const float square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
if (1.0f - BG_FP32_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP32_TWO_EPSYLON) {
return;
}
if (square_modulus <= BG_FP32_SQUARE_EPSYLON) {
twin->s0 = 1.0f; twin->s0 = 1.0f;
twin->x1 = 0.0f; twin->x1 = 0.0f;
twin->x2 = 0.0f; twin->x2 = 0.0f;
@ -17,17 +32,30 @@ void __bg_fp32_versor_normalize(const float square_modulus, __BgFP32DarkTwinVers
return; return;
} }
const float modulus = sqrtf(square_modulus); const float multiplier = sqrtf(1.0f / square_modulus);
twin->s0 /= modulus; twin->s0 *= multiplier;
twin->x1 /= modulus; twin->x1 *= multiplier;
twin->x2 /= modulus; twin->x2 *= multiplier;
twin->x3 /= modulus; twin->x3 *= multiplier;
} }
void __bg_fp64_versor_normalize(const double square_modulus, __BgFP64DarkTwinVersor* twin) void bg_fp64_versor_set_values(const double s0, const double x1, const double x2, const double x3, BgFP64Versor* versor)
{ {
if (square_modulus <= BG_FP64_SQUARE_EPSYLON || (twin->s0 * twin->s0) >= (1.0 - BG_FP64_TWO_EPSYLON) * square_modulus) { __BgFP64DarkTwinVersor* twin = (__BgFP64DarkTwinVersor*)versor;
twin->s0 = s0;
twin->x1 = x1;
twin->x2 = x2;
twin->x3 = x3;
const double square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
if (1.0 - BG_FP64_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0 + BG_FP64_TWO_EPSYLON) {
return;
}
if (square_modulus <= BG_FP64_SQUARE_EPSYLON) {
twin->s0 = 1.0; twin->s0 = 1.0;
twin->x1 = 0.0; twin->x1 = 0.0;
twin->x2 = 0.0; twin->x2 = 0.0;
@ -35,12 +63,12 @@ void __bg_fp64_versor_normalize(const double square_modulus, __BgFP64DarkTwinVer
return; return;
} }
const double modulus = sqrt(square_modulus); const double multiplier = sqrt(1.0 / square_modulus);
twin->s0 /= modulus; twin->s0 *= multiplier;
twin->x1 /= modulus; twin->x1 *= multiplier;
twin->x2 /= modulus; twin->x2 *= multiplier;
twin->x3 /= modulus; twin->x3 *= multiplier;
} }
// =============== Set Crude Turn =============== // // =============== Set Crude Turn =============== //
@ -108,11 +136,11 @@ void bg_fp32_versor_get_rotation(const BgFP32Versor* versor, BgFP32Rotation3* re
result->radians = 2.0f * acosf(versor->s0 / sqrtf(versor->s0 * versor->s0 + square_vector)); result->radians = 2.0f * acosf(versor->s0 / sqrtf(versor->s0 * versor->s0 + square_vector));
const float vector_modulus = sqrtf(square_vector); const float multiplier = sqrtf(1.0f / square_vector);
result->axis.x1 = versor->x1 / vector_modulus; result->axis.x1 = versor->x1 * multiplier;
result->axis.x2 = versor->x2 / vector_modulus; result->axis.x2 = versor->x2 * multiplier;
result->axis.x3 = versor->x3 / vector_modulus; result->axis.x3 = versor->x3 * multiplier;
} }
void bg_fp64_versor_get_rotation(const BgFP64Versor* versor, BgFP64Rotation3* result) void bg_fp64_versor_get_rotation(const BgFP64Versor* versor, BgFP64Rotation3* result)
@ -130,9 +158,264 @@ void bg_fp64_versor_get_rotation(const BgFP64Versor* versor, BgFP64Rotation3* re
result->radians = 2.0 * acos(versor->s0 / sqrt(versor->s0 * versor->s0 + square_vector)); result->radians = 2.0 * acos(versor->s0 / sqrt(versor->s0 * versor->s0 + square_vector));
const double vector_modulus = sqrt(square_vector); const double multiplier = sqrt(1.0 / square_vector);
result->axis.x1 = versor->x1 / vector_modulus; result->axis.x1 = versor->x1 * multiplier;
result->axis.x2 = versor->x2 / vector_modulus; result->axis.x2 = versor->x2 * multiplier;
result->axis.x3 = versor->x3 / vector_modulus; result->axis.x3 = versor->x3 * multiplier;
} }
// ================ Combination ================= //
void bg_fp32_versor_combine(const BgFP32Versor* second, const BgFP32Versor* first, BgFP32Versor* 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);
__BgFP32DarkTwinVersor* twin = (__BgFP32DarkTwinVersor*)result;
twin->s0 = s0;
twin->x1 = x1;
twin->x2 = x2;
twin->x3 = x3;
if (1.0f - BG_FP32_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP32_TWO_EPSYLON) {
return;
}
if (square_modulus <= BG_FP32_SQUARE_EPSYLON) {
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;
}
void bg_fp64_versor_combine(const BgFP64Versor* second, const BgFP64Versor* first, BgFP64Versor* 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);
__BgFP64DarkTwinVersor* twin = (__BgFP64DarkTwinVersor*)result;
twin->s0 = s0;
twin->x1 = x1;
twin->x2 = x2;
twin->x3 = x3;
if (1.0 - BG_FP64_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0 + BG_FP64_TWO_EPSYLON) {
return;
}
if (square_modulus <= BG_FP64_SQUARE_EPSYLON) {
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;
}
// =========== Make Rotation Matrix3x3 ========== //
void bg_fp32_versor_get_rotation_matrix(const BgFP32Versor* versor, BgFP32Matrix3x3* matrix)
{
const float s0s0 = versor->s0 * versor->s0;
const float x1x1 = versor->x1 * versor->x1;
const float x2x2 = versor->x2 * versor->x2;
const float x3x3 = versor->x3 * versor->x3;
const float s0x1 = 2.0f * versor->s0 * versor->x1;
const float s0x2 = 2.0f * versor->s0 * versor->x2;
const float s0x3 = 2.0f * versor->s0 * versor->x3;
const float x1x2 = 2.0f * versor->x1 * versor->x2;
const float x1x3 = 2.0f * versor->x1 * versor->x3;
const float x2x3 = 2.0f * versor->x2 * versor->x3;
matrix->r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3);
matrix->r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3);
matrix->r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2);
matrix->r1c2 = x1x2 - s0x3;
matrix->r2c3 = x2x3 - s0x1;
matrix->r3c1 = x1x3 - s0x2;
matrix->r2c1 = x1x2 + s0x3;
matrix->r3c2 = x2x3 + s0x1;
matrix->r1c3 = x1x3 + s0x2;
}
void bg_fp64_versor_get_rotation_matrix(const BgFP64Versor* versor, BgFP64Matrix3x3* matrix)
{
const double s0s0 = versor->s0 * versor->s0;
const double x1x1 = versor->x1 * versor->x1;
const double x2x2 = versor->x2 * versor->x2;
const double x3x3 = versor->x3 * versor->x3;
const double s0x1 = 2.0 * versor->s0 * versor->x1;
const double s0x2 = 2.0 * versor->s0 * versor->x2;
const double s0x3 = 2.0 * versor->s0 * versor->x3;
const double x1x2 = 2.0 * versor->x1 * versor->x2;
const double x1x3 = 2.0 * versor->x1 * versor->x3;
const double x2x3 = 2.0 * versor->x2 * versor->x3;
matrix->r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3);
matrix->r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3);
matrix->r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2);
matrix->r1c2 = x1x2 - s0x3;
matrix->r2c3 = x2x3 - s0x1;
matrix->r3c1 = x1x3 - s0x2;
matrix->r2c1 = x1x2 + s0x3;
matrix->r3c2 = x2x3 + s0x1;
matrix->r1c3 = x1x3 + s0x2;
}
// =========== Make Reverse Matrix3x3 =========== //
void bg_fp32_versor_get_reverse_matrix(const BgFP32Versor* versor, BgFP32Matrix3x3* matrix)
{
const float s0s0 = versor->s0 * versor->s0;
const float x1x1 = versor->x1 * versor->x1;
const float x2x2 = versor->x2 * versor->x2;
const float x3x3 = versor->x3 * versor->x3;
const float s0x1 = 2.0f * versor->s0 * versor->x1;
const float s0x2 = 2.0f * versor->s0 * versor->x2;
const float s0x3 = 2.0f * versor->s0 * versor->x3;
const float x1x2 = 2.0f * versor->x1 * versor->x2;
const float x1x3 = 2.0f * versor->x1 * versor->x3;
const float x2x3 = 2.0f * versor->x2 * versor->x3;
matrix->r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3);
matrix->r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3);
matrix->r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2);
matrix->r1c2 = x1x2 + s0x3;
matrix->r2c3 = x2x3 + s0x1;
matrix->r3c1 = x1x3 + s0x2;
matrix->r2c1 = x1x2 - s0x3;
matrix->r3c2 = x2x3 - s0x1;
matrix->r1c3 = x1x3 - s0x2;
}
void bg_fp64_versor_get_reverse_matrix(const BgFP64Versor* versor, BgFP64Matrix3x3* matrix)
{
const double s0s0 = versor->s0 * versor->s0;
const double x1x1 = versor->x1 * versor->x1;
const double x2x2 = versor->x2 * versor->x2;
const double x3x3 = versor->x3 * versor->x3;
const double s0x1 = 2.0 * versor->s0 * versor->x1;
const double s0x2 = 2.0 * versor->s0 * versor->x2;
const double s0x3 = 2.0 * versor->s0 * versor->x3;
const double x1x2 = 2.0 * versor->x1 * versor->x2;
const double x1x3 = 2.0 * versor->x1 * versor->x3;
const double x2x3 = 2.0 * versor->x2 * versor->x3;
matrix->r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3);
matrix->r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3);
matrix->r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2);
matrix->r1c2 = x1x2 + s0x3;
matrix->r2c3 = x2x3 + s0x1;
matrix->r3c1 = x1x3 + s0x2;
matrix->r2c1 = x1x2 - s0x3;
matrix->r3c2 = x2x3 - s0x1;
matrix->r1c3 = x1x3 - s0x2;
}
// ================ Turn Vector ================= //
void bg_fp32_versor_turn(const BgFP32Versor* versor, const BgFP32Vector3* vector, BgFP32Vector3* result)
{
const float tx1 = 2.0f * (versor->x2 * vector->x3 - versor->x3 * vector->x2);
const float tx2 = 2.0f * (versor->x3 * vector->x1 - versor->x1 * vector->x3);
const float tx3 = 2.0f * (versor->x1 * vector->x2 - versor->x2 * vector->x1);
const float x1 = (vector->x1 + tx1 * versor->s0) + (versor->x2 * tx3 - versor->x3 * tx2);
const float x2 = (vector->x2 + tx2 * versor->s0) + (versor->x3 * tx1 - versor->x1 * tx3);
const float x3 = (vector->x3 + tx3 * versor->s0) + (versor->x1 * tx2 - versor->x2 * tx1);
result->x1 = x1;
result->x2 = x2;
result->x3 = x3;
}
void bg_fp64_versor_turn(const BgFP64Versor* versor, const BgFP64Vector3* vector, BgFP64Vector3* result)
{
const double tx1 = 2.0 * (versor->x2 * vector->x3 - versor->x3 * vector->x2);
const double tx2 = 2.0 * (versor->x3 * vector->x1 - versor->x1 * vector->x3);
const double tx3 = 2.0 * (versor->x1 * vector->x2 - versor->x2 * vector->x1);
const double x1 = (vector->x1 + tx1 * versor->s0) + (versor->x2 * tx3 - versor->x3 * tx2);
const double x2 = (vector->x2 + tx2 * versor->s0) + (versor->x3 * tx1 - versor->x1 * tx3);
const double x3 = (vector->x3 + tx3 * versor->s0) + (versor->x1 * tx2 - versor->x2 * tx1);
result->x1 = x1;
result->x2 = x2;
result->x3 = x3;
}
// ============== Turn Vector Back ============== //
void bg_fp32_versor_turn_back(const BgFP32Versor* versor, const BgFP32Vector3* vector, BgFP32Vector3* result)
{
const float tx1 = 2.0f * (versor->x2 * vector->x3 - versor->x3 * vector->x2);
const float tx2 = 2.0f * (versor->x3 * vector->x1 - versor->x1 * vector->x3);
const float tx3 = 2.0f * (versor->x1 * vector->x2 - versor->x2 * vector->x1);
const float x1 = (vector->x1 - tx1 * versor->s0) + (versor->x2 * tx3 - versor->x3 * tx2);
const float x2 = (vector->x2 - tx2 * versor->s0) + (versor->x3 * tx1 - versor->x1 * tx3);
const float x3 = (vector->x3 - tx3 * versor->s0) + (versor->x1 * tx2 - versor->x2 * tx1);
result->x1 = x1;
result->x2 = x2;
result->x3 = x3;
}
void bg_fp64_versor_turn_back(const BgFP64Versor* versor, const BgFP64Vector3* vector, BgFP64Vector3* result)
{
const double tx1 = 2.0 * (versor->x2 * vector->x3 - versor->x3 * vector->x2);
const double tx2 = 2.0 * (versor->x3 * vector->x1 - versor->x1 * vector->x3);
const double tx3 = 2.0 * (versor->x1 * vector->x2 - versor->x2 * vector->x1);
const double x1 = (vector->x1 - tx1 * versor->s0) + (versor->x2 * tx3 - versor->x3 * tx2);
const double x2 = (vector->x2 - tx2 * versor->s0) + (versor->x3 * tx1 - versor->x1 * tx3);
const double x3 = (vector->x3 - tx3 * versor->s0) + (versor->x1 * tx2 - versor->x2 * tx1);
result->x1 = x1;
result->x2 = x2;
result->x3 = x3;
}

414
basic-geometry/versor.h
View file

@ -58,45 +58,9 @@ static inline void bg_fp64_versor_reset(BgFP64Versor* versor)
// ==================== Set ===================== // // ==================== Set ===================== //
void __bg_fp32_versor_normalize(const float square_modulus, __BgFP32DarkTwinVersor* twin); void bg_fp32_versor_set_values(const float s0, const float x1, const float x2, const float x3, BgFP32Versor* versor);
void __bg_fp64_versor_normalize(const double square_modulus, __BgFP64DarkTwinVersor* twin); void bg_fp64_versor_set_values(const double s0, const double x1, const double x2, const double x3, BgFP64Versor* versor);
static inline void bg_fp32_versor_set_values(const float s0, const float x1, const float x2, const float x3, BgFP32Versor* versor)
{
__BgFP32DarkTwinVersor* twin = (__BgFP32DarkTwinVersor*)versor;
twin->s0 = s0;
twin->x1 = x1;
twin->x2 = x2;
twin->x3 = x3;
const float square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
if (1.0f - BG_FP32_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP32_TWO_EPSYLON) {
return;
}
__bg_fp32_versor_normalize(square_modulus, (__BgFP32DarkTwinVersor*)versor);
}
static inline void bg_fp64_versor_set_values(const double s0, const double x1, const double x2, const double x3, BgFP64Versor* versor)
{
__BgFP64DarkTwinVersor* twin = (__BgFP64DarkTwinVersor*)versor;
twin->s0 = s0;
twin->x1 = x1;
twin->x2 = x2;
twin->x3 = x3;
const double square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
if (1.0 - BG_FP64_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0 + BG_FP64_TWO_EPSYLON) {
return;
}
__bg_fp64_versor_normalize(square_modulus, twin);
}
// ==================== Copy ==================== // // ==================== Copy ==================== //
@ -152,24 +116,24 @@ static inline void bg_fp64_versor_set_rotation(const BgFP64Rotation3* rotation,
// =============== Square modulus =============== // // =============== Square modulus =============== //
static inline int bg_fp32_versor_get_square_modulus(const BgFP32Versor* versor) static inline float bg_fp32_versor_get_square_modulus(const BgFP32Versor* versor)
{ {
return (versor->s0 * versor->s0 + versor->x1 * versor->x1) + (versor->x2 * versor->x2 + versor->x3 * versor->x3); return (versor->s0 * versor->s0 + versor->x1 * versor->x1) + (versor->x2 * versor->x2 + versor->x3 * versor->x3);
} }
static inline int bg_fp64_versor_get_square_modulus(const BgFP64Versor* versor) static inline double bg_fp64_versor_get_square_modulus(const BgFP64Versor* versor)
{ {
return (versor->s0 * versor->s0 + versor->x1 * versor->x1) + (versor->x2 * versor->x2 + versor->x3 * versor->x3); return (versor->s0 * versor->s0 + versor->x1 * versor->x1) + (versor->x2 * versor->x2 + versor->x3 * versor->x3);
} }
// =================== Modulus ================== // // =================== Modulus ================== //
static inline int bg_fp32_versor_get_modulus(const BgFP32Versor* versor) static inline float bg_fp32_versor_get_modulus(const BgFP32Versor* versor)
{ {
return sqrtf(bg_fp32_versor_get_square_modulus(versor)); return sqrtf(bg_fp32_versor_get_square_modulus(versor));
} }
static inline int bg_fp64_versor_get_modulus(const BgFP64Versor* versor) static inline double bg_fp64_versor_get_modulus(const BgFP64Versor* versor)
{ {
return sqrt(bg_fp64_versor_get_square_modulus(versor)); return sqrt(bg_fp64_versor_get_square_modulus(versor));
} }
@ -274,51 +238,9 @@ static inline void bg_fp64_versor_set_inverted_fp32(const BgFP32Versor* versor,
// ================ Combination ================= // // ================ Combination ================= //
static inline void bg_fp32_versor_combine(const BgFP32Versor* second, const BgFP32Versor* first, BgFP32Versor* result) void bg_fp32_versor_combine(const BgFP32Versor* second, const BgFP32Versor* first, BgFP32Versor* 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); void bg_fp64_versor_combine(const BgFP64Versor* second, const BgFP64Versor* first, BgFP64Versor* result);
__BgFP32DarkTwinVersor* twin = (__BgFP32DarkTwinVersor*)result;
twin->s0 = s0;
twin->x1 = x1;
twin->x2 = x2;
twin->x3 = x3;
if (1.0f - BG_FP32_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP32_TWO_EPSYLON) {
return;
}
__bg_fp32_versor_normalize(square_modulus, twin);
}
static inline void bg_fp64_versor_combine(const BgFP64Versor* second, const BgFP64Versor* first, BgFP64Versor* 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);
__BgFP64DarkTwinVersor* twin = (__BgFP64DarkTwinVersor*)result;
twin->s0 = s0;
twin->x1 = x1;
twin->x2 = x2;
twin->x3 = x3;
if (1.0 - BG_FP64_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0 + BG_FP64_TWO_EPSYLON) {
return;
}
__bg_fp64_versor_normalize(square_modulus, twin);
}
// ================= Rotation3 ================== // // ================= Rotation3 ================== //
@ -328,330 +250,26 @@ void bg_fp64_versor_get_rotation(const BgFP64Versor* versor, BgFP64Rotation3* re
// =========== Make Rotation Matrix3x3 ========== // // =========== Make Rotation Matrix3x3 ========== //
static inline void bg_fp32_versor_get_rotation_matrix(const BgFP32Versor* versor, BgFP32Matrix3x3* matrix) void bg_fp32_versor_get_rotation_matrix(const BgFP32Versor* versor, BgFP32Matrix3x3* matrix);
{
const float s0s0 = versor->s0 * versor->s0;
const float x1x1 = versor->x1 * versor->x1;
const float x2x2 = versor->x2 * versor->x2;
const float x3x3 = versor->x3 * versor->x3;
const float s0x1 = 2.0f * versor->s0 * versor->x1; void bg_fp64_versor_get_rotation_matrix(const BgFP64Versor* versor, BgFP64Matrix3x3* matrix);
const float s0x2 = 2.0f * versor->s0 * versor->x2;
const float s0x3 = 2.0f * versor->s0 * versor->x3;
const float x1x2 = 2.0f * versor->x1 * versor->x2;
const float x1x3 = 2.0f * versor->x1 * versor->x3;
const float x2x3 = 2.0f * versor->x2 * versor->x3;
matrix->r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3);
matrix->r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3);
matrix->r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2);
matrix->r1c2 = x1x2 - s0x3;
matrix->r2c3 = x2x3 - s0x1;
matrix->r3c1 = x1x3 - s0x2;
matrix->r2c1 = x1x2 + s0x3;
matrix->r3c2 = x2x3 + s0x1;
matrix->r1c3 = x1x3 + s0x2;
}
static inline void bg_fp64_versor_get_rotation_matrix(const BgFP64Versor* versor, BgFP64Matrix3x3* matrix)
{
const double s0s0 = versor->s0 * versor->s0;
const double x1x1 = versor->x1 * versor->x1;
const double x2x2 = versor->x2 * versor->x2;
const double x3x3 = versor->x3 * versor->x3;
const double s0x1 = 2.0 * versor->s0 * versor->x1;
const double s0x2 = 2.0 * versor->s0 * versor->x2;
const double s0x3 = 2.0 * versor->s0 * versor->x3;
const double x1x2 = 2.0 * versor->x1 * versor->x2;
const double x1x3 = 2.0 * versor->x1 * versor->x3;
const double x2x3 = 2.0 * versor->x2 * versor->x3;
matrix->r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3);
matrix->r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3);
matrix->r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2);
matrix->r1c2 = x1x2 - s0x3;
matrix->r2c3 = x2x3 - s0x1;
matrix->r3c1 = x1x3 - s0x2;
matrix->r2c1 = x1x2 + s0x3;
matrix->r3c2 = x2x3 + s0x1;
matrix->r1c3 = x1x3 + s0x2;
}
// =========== Make Reverse Matrix3x3 =========== // // =========== Make Reverse Matrix3x3 =========== //
static inline void bg_fp32_versor_get_reverse_matrix(const BgFP32Versor* versor, BgFP32Matrix3x3* matrix) void bg_fp32_versor_get_reverse_matrix(const BgFP32Versor* versor, BgFP32Matrix3x3* matrix);
{
const float s0s0 = versor->s0 * versor->s0;
const float x1x1 = versor->x1 * versor->x1;
const float x2x2 = versor->x2 * versor->x2;
const float x3x3 = versor->x3 * versor->x3;
const float s0x1 = 2.0f * versor->s0 * versor->x1; void bg_fp64_versor_get_reverse_matrix(const BgFP64Versor* versor, BgFP64Matrix3x3* matrix);
const float s0x2 = 2.0f * versor->s0 * versor->x2;
const float s0x3 = 2.0f * versor->s0 * versor->x3;
const float x1x2 = 2.0f * versor->x1 * versor->x2;
const float x1x3 = 2.0f * versor->x1 * versor->x3;
const float x2x3 = 2.0f * versor->x2 * versor->x3;
matrix->r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3);
matrix->r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3);
matrix->r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2);
matrix->r1c2 = x1x2 + s0x3;
matrix->r2c3 = x2x3 + s0x1;
matrix->r3c1 = x1x3 + s0x2;
matrix->r2c1 = x1x2 - s0x3;
matrix->r3c2 = x2x3 - s0x1;
matrix->r1c3 = x1x3 - s0x2;
}
static inline void bg_fp64_versor_get_reverse_matrix(const BgFP64Versor* versor, BgFP64Matrix3x3* matrix)
{
const double s0s0 = versor->s0 * versor->s0;
const double x1x1 = versor->x1 * versor->x1;
const double x2x2 = versor->x2 * versor->x2;
const double x3x3 = versor->x3 * versor->x3;
const double s0x1 = 2.0 * versor->s0 * versor->x1;
const double s0x2 = 2.0 * versor->s0 * versor->x2;
const double s0x3 = 2.0 * versor->s0 * versor->x3;
const double x1x2 = 2.0 * versor->x1 * versor->x2;
const double x1x3 = 2.0 * versor->x1 * versor->x3;
const double x2x3 = 2.0 * versor->x2 * versor->x3;
matrix->r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3);
matrix->r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3);
matrix->r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2);
matrix->r1c2 = x1x2 + s0x3;
matrix->r2c3 = x2x3 + s0x1;
matrix->r3c1 = x1x3 + s0x2;
matrix->r2c1 = x1x2 - s0x3;
matrix->r3c2 = x2x3 - s0x1;
matrix->r1c3 = x1x3 - s0x2;
}
// ================ Turn Vector ================= // // ================ Turn Vector ================= //
static inline void bg_fp32_versor_turn(const BgFP32Versor* versor, const BgFP32Vector3* vector, BgFP32Vector3* result) void bg_fp32_versor_turn(const BgFP32Versor* versor, const BgFP32Vector3* vector, BgFP32Vector3* result);
{
const float tx1 = 2.0f * (versor->x2 * vector->x3 - versor->x3 * vector->x2);
const float tx2 = 2.0f * (versor->x3 * vector->x1 - versor->x1 * vector->x3);
const float tx3 = 2.0f * (versor->x1 * vector->x2 - versor->x2 * vector->x1);
const float x1 = (vector->x1 + tx1 * versor->s0) + (versor->x2 * tx3 - versor->x3 * tx2); void bg_fp64_versor_turn(const BgFP64Versor* versor, const BgFP64Vector3* vector, BgFP64Vector3* result);
const float x2 = (vector->x2 + tx2 * versor->s0) + (versor->x3 * tx1 - versor->x1 * tx3);
const float x3 = (vector->x3 + tx3 * versor->s0) + (versor->x1 * tx2 - versor->x2 * tx1);
result->x1 = x1;
result->x2 = x2;
result->x3 = x3;
}
static inline void bg_fp64_versor_turn(const BgFP64Versor* versor, const BgFP64Vector3* vector, BgFP64Vector3* result)
{
const double tx1 = 2.0 * (versor->x2 * vector->x3 - versor->x3 * vector->x2);
const double tx2 = 2.0 * (versor->x3 * vector->x1 - versor->x1 * vector->x3);
const double tx3 = 2.0 * (versor->x1 * vector->x2 - versor->x2 * vector->x1);
const double x1 = (vector->x1 + tx1 * versor->s0) + (versor->x2 * tx3 - versor->x3 * tx2);
const double x2 = (vector->x2 + tx2 * versor->s0) + (versor->x3 * tx1 - versor->x1 * tx3);
const double x3 = (vector->x3 + tx3 * versor->s0) + (versor->x1 * tx2 - versor->x2 * tx1);
result->x1 = x1;
result->x2 = x2;
result->x3 = x3;
}
// ================ Turn2 Vector ================ //
static inline void bg_fp32_versor_turn2(const BgFP32Versor* versor, const BgFP32Vector3* vector, BgFP32Vector3* result)
{
const float s0s0 = versor->s0 * versor->s0;
const float x1x1 = versor->x1 * versor->x1;
const float x2x2 = versor->x2 * versor->x2;
const float x3x3 = versor->x3 * versor->x3;
const float s0x1 = 2.0f * versor->s0 * versor->x1;
const float s0x2 = 2.0f * versor->s0 * versor->x2;
const float s0x3 = 2.0f * versor->s0 * versor->x3;
const float x1x2 = 2.0f * versor->x1 * versor->x2;
const float x1x3 = 2.0f * versor->x1 * versor->x3;
const float x2x3 = 2.0f * versor->x2 * versor->x3;
const float r2c1 = x1x2 + s0x3;
const float r3c2 = x2x3 + s0x1;
const float r1c3 = x1x3 + s0x2;
const float r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3);
const float r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3);
const float r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2);
const float r1c2 = x1x2 - s0x3;
const float r2c3 = x2x3 - s0x1;
const float r3c1 = x1x3 - s0x2;
const float x1 = r1c1 * vector->x1 + r1c2 * vector->x2 + r1c3 * vector->x3;
const float x2 = r2c1 * vector->x1 + r2c2 * vector->x2 + r2c3 * vector->x3;
const float x3 = r3c1 * vector->x1 + r3c2 * vector->x2 + r3c3 * vector->x3;
result->x1 = x1;
result->x2 = x2;
result->x3 = x3;
}
static inline void bg_fp64_versor_turn2(const BgFP64Versor* versor, const BgFP64Vector3* vector, BgFP64Vector3* result)
{
const double s0s0 = versor->s0 * versor->s0;
const double x1x1 = versor->x1 * versor->x1;
const double x2x2 = versor->x2 * versor->x2;
const double x3x3 = versor->x3 * versor->x3;
const double s0x1 = 2.0f * versor->s0 * versor->x1;
const double s0x2 = 2.0f * versor->s0 * versor->x2;
const double s0x3 = 2.0f * versor->s0 * versor->x3;
const double x1x2 = 2.0f * versor->x1 * versor->x2;
const double x1x3 = 2.0f * versor->x1 * versor->x3;
const double x2x3 = 2.0f * versor->x2 * versor->x3;
const double r2c1 = x1x2 + s0x3;
const double r3c2 = x2x3 + s0x1;
const double r1c3 = x1x3 + s0x2;
const double r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3);
const double r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3);
const double r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2);
const double r1c2 = x1x2 - s0x3;
const double r2c3 = x2x3 - s0x1;
const double r3c1 = x1x3 - s0x2;
const double x1 = r1c1 * vector->x1 + r1c2 * vector->x2 + r1c3 * vector->x3;
const double x2 = r2c1 * vector->x1 + r2c2 * vector->x2 + r2c3 * vector->x3;
const double x3 = r3c1 * vector->x1 + r3c2 * vector->x2 + r3c3 * vector->x3;
result->x1 = x1;
result->x2 = x2;
result->x3 = x3;
}
// ============== Turn Vector Back ============== // // ============== Turn Vector Back ============== //
static inline void bg_fp32_versor_turn_back(const BgFP32Versor* versor, const BgFP32Vector3* vector, BgFP32Vector3* result) void bg_fp32_versor_turn_back(const BgFP32Versor* versor, const BgFP32Vector3* vector, BgFP32Vector3* result);
{
const float tx1 = 2.0f * (versor->x2 * vector->x3 - versor->x3 * vector->x2);
const float tx2 = 2.0f * (versor->x3 * vector->x1 - versor->x1 * vector->x3);
const float tx3 = 2.0f * (versor->x1 * vector->x2 - versor->x2 * vector->x1);
const float x1 = (vector->x1 - tx1 * versor->s0) + (versor->x2 * tx3 - versor->x3 * tx2); void bg_fp64_versor_turn_back(const BgFP64Versor* versor, const BgFP64Vector3* vector, BgFP64Vector3* result);
const float x2 = (vector->x2 - tx2 * versor->s0) + (versor->x3 * tx1 - versor->x1 * tx3);
const float x3 = (vector->x3 - tx3 * versor->s0) + (versor->x1 * tx2 - versor->x2 * tx1);
result->x1 = x1;
result->x2 = x2;
result->x3 = x3;
}
static inline void bg_fp64_versor_turn_back(const BgFP64Versor* versor, const BgFP64Vector3* vector, BgFP64Vector3* result)
{
const double tx1 = 2.0 * (versor->x2 * vector->x3 - versor->x3 * vector->x2);
const double tx2 = 2.0 * (versor->x3 * vector->x1 - versor->x1 * vector->x3);
const double tx3 = 2.0 * (versor->x1 * vector->x2 - versor->x2 * vector->x1);
const double x1 = (vector->x1 - tx1 * versor->s0) + (versor->x2 * tx3 - versor->x3 * tx2);
const double x2 = (vector->x2 - tx2 * versor->s0) + (versor->x3 * tx1 - versor->x1 * tx3);
const double x3 = (vector->x3 - tx3 * versor->s0) + (versor->x1 * tx2 - versor->x2 * tx1);
result->x1 = x1;
result->x2 = x2;
result->x3 = x3;
}
// ============== Turn Vector Back2 ============= //
static inline void bg_fp32_versor_turn_back2(const BgFP32Versor* versor, const BgFP32Vector3* vector, BgFP32Vector3* result)
{
const float s0s0 = versor->s0 * versor->s0;
const float x1x1 = versor->x1 * versor->x1;
const float x2x2 = versor->x2 * versor->x2;
const float x3x3 = versor->x3 * versor->x3;
const float s0x1 = 2.0f * versor->s0 * versor->x1;
const float s0x2 = 2.0f * versor->s0 * versor->x2;
const float s0x3 = 2.0f * versor->s0 * versor->x3;
const float x1x2 = 2.0f * versor->x1 * versor->x2;
const float x1x3 = 2.0f * versor->x1 * versor->x3;
const float x2x3 = 2.0f * versor->x2 * versor->x3;
const float r1c2 = x1x2 + s0x3;
const float r2c3 = x2x3 + s0x1;
const float r3c1 = x1x3 + s0x2;
const float r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3);
const float r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3);
const float r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2);
const float r2c1 = x1x2 - s0x3;
const float r3c2 = x2x3 - s0x1;
const float r1c3 = x1x3 - s0x2;
const float x1 = r1c1 * vector->x1 + r1c2 * vector->x2 + r1c3 * vector->x3;
const float x2 = r2c1 * vector->x1 + r2c2 * vector->x2 + r2c3 * vector->x3;
const float x3 = r3c1 * vector->x1 + r3c2 * vector->x2 + r3c3 * vector->x3;
result->x1 = x1;
result->x2 = x2;
result->x3 = x3;
}
static inline void bg_fp64_versor_turn_back2(const BgFP64Versor* versor, const BgFP64Vector3* vector, BgFP64Vector3* result)
{
const double s0s0 = versor->s0 * versor->s0;
const double x1x1 = versor->x1 * versor->x1;
const double x2x2 = versor->x2 * versor->x2;
const double x3x3 = versor->x3 * versor->x3;
const double s0x1 = 2.0f * versor->s0 * versor->x1;
const double s0x2 = 2.0f * versor->s0 * versor->x2;
const double s0x3 = 2.0f * versor->s0 * versor->x3;
const double x1x2 = 2.0f * versor->x1 * versor->x2;
const double x1x3 = 2.0f * versor->x1 * versor->x3;
const double x2x3 = 2.0f * versor->x2 * versor->x3;
const double r1c2 = x1x2 + s0x3;
const double r2c3 = x2x3 + s0x1;
const double r3c1 = x1x3 + s0x2;
const double r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3);
const double r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3);
const double r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2);
const double r2c1 = x1x2 - s0x3;
const double r3c2 = x2x3 - s0x1;
const double r1c3 = x1x3 - s0x2;
const double x1 = r1c1 * vector->x1 + r1c2 * vector->x2 + r1c3 * vector->x3;
const double x2 = r2c1 * vector->x1 + r2c2 * vector->x2 + r2c3 * vector->x3;
const double x3 = r3c1 * vector->x1 + r3c2 * vector->x2 + r3c3 * vector->x3;
result->x1 = x1;
result->x2 = x2;
result->x3 = x3;
}
#endif #endif