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-02-26 00:25:17 +07:00
parent fa9ecda57b
commit 34ee460873
21 changed files with 976 additions and 1158 deletions
Download patch file Download diff file Expand all files Collapse all files

2
basic-geometry-test/tests/vector2/vector2_is_zero.c
View file

@ -21,7 +21,7 @@ static const BgcVector2FP32 _TEST_FP32_NONZERO_VECTOR2_LIST[] = {
{ -1.25f * BGC_EPSYLON_FP32, 0.0f },
{ 0.0f, 1.25f * BGC_EPSYLON_FP32 },
{ 0.0f, -1.25f * BGC_EPSYLON_FP32 },
{ 1.25f * BGC_EPSYLON_FP32, 1.25 * BGC_EPSYLON_FP32 },
{ 1.25f * BGC_EPSYLON_FP32, 1.25f * BGC_EPSYLON_FP32 },
{ -1.25f * BGC_EPSYLON_FP32, -1.25f * BGC_EPSYLON_FP32 }
};

23
basic-geometry/matrix2x2.c
View file

@ -18,20 +18,20 @@ extern inline double bgc_matrix2x2_get_determinant_fp64(const BgcMatrix2x2FP64*
extern inline int bgc_matrix2x2_is_singular_fp32(const BgcMatrix2x2FP32* matrix);
extern inline int bgc_matrix2x2_is_singular_fp64(const BgcMatrix2x2FP64* matrix);
extern inline void bgc_matrix2x2_copy_fp32(const BgcMatrix2x2FP32* from, BgcMatrix2x2FP32* to);
extern inline void bgc_matrix2x2_copy_fp64(const BgcMatrix2x2FP64* from, BgcMatrix2x2FP64* to);
extern inline void bgc_matrix2x2_copy_fp32(const BgcMatrix2x2FP32* source, BgcMatrix2x2FP32* destination);
extern inline void bgc_matrix2x2_copy_fp64(const BgcMatrix2x2FP64* source, BgcMatrix2x2FP64* destination);
extern inline void bgc_matrix2x2_swap_fp32(BgcMatrix2x2FP32* matrix1, BgcMatrix2x2FP32* matrix2);
extern inline void bgc_matrix2x2_swap_fp64(BgcMatrix2x2FP64* matrix1, BgcMatrix2x2FP64* matrix2);
extern inline void bgc_matrix2x2_convert_fp64_to_fp32(const BgcMatrix2x2FP64* from, BgcMatrix2x2FP32* to);
extern inline void bgc_matrix2x2_convert_fp32_to_fp64(const BgcMatrix2x2FP32* from, BgcMatrix2x2FP64* to);
extern inline void bgc_matrix2x2_convert_fp64_to_fp32(const BgcMatrix2x2FP64* source, BgcMatrix2x2FP32* destination);
extern inline void bgc_matrix2x2_convert_fp32_to_fp64(const BgcMatrix2x2FP32* source, BgcMatrix2x2FP64* destination);
extern inline int bgc_matrix2x2_invert_fp32(BgcMatrix2x2FP32* matrix);
extern inline int bgc_matrix2x2_invert_fp64(BgcMatrix2x2FP64* matrix);
extern inline int bgc_matrix2x2_invert_fp32(const BgcMatrix2x2FP32* matrix, BgcMatrix2x2FP32* inverted);
extern inline int bgc_matrix2x2_invert_fp64(const BgcMatrix2x2FP64* matrix, BgcMatrix2x2FP64* inverted);
extern inline void bgc_matrix2x2_transpose_fp32(BgcMatrix2x2FP32* matrix);
extern inline void bgc_matrix2x2_transpose_fp64(BgcMatrix2x2FP64* matrix);
extern inline void bgc_matrix2x2_transpose_fp32(const BgcMatrix2x2FP32* matrix, BgcMatrix2x2FP32* transposed);
extern inline void bgc_matrix2x2_transpose_fp64(const BgcMatrix2x2FP64* matrix, BgcMatrix2x2FP64* transposed);
extern inline void bgc_matrix2x2_set_row1_fp32(const float c1, const float c2, BgcMatrix2x2FP32* matrix);
extern inline void bgc_matrix2x2_set_row1_fp64(const double c1, const double c2, BgcMatrix2x2FP64* matrix);
@ -45,12 +45,6 @@ extern inline void bgc_matrix2x2_set_column1_fp64(const double r1, const double
extern inline void bgc_matrix2x2_set_column2_fp32(const float r1, const float r2, BgcMatrix2x2FP32* matrix);
extern inline void bgc_matrix2x2_set_column2_fp64(const double r1, const double r2, BgcMatrix2x2FP64* matrix);
extern inline int bgc_matrix2x2_get_inverted_fp32(const BgcMatrix2x2FP32* from, BgcMatrix2x2FP32* to);
extern inline int bgc_matrix2x2_get_inverted_fp64(const BgcMatrix2x2FP64* from, BgcMatrix2x2FP64* to);
extern inline void bgc_matrix2x2_get_transposed_fp32(const BgcMatrix2x2FP32* from, BgcMatrix2x2FP32* to);
extern inline void bgc_matrix2x2_get_transposed_fp64(const BgcMatrix2x2FP64* from, BgcMatrix2x2FP64* to);
extern inline void bgc_matrix2x2_add_fp32(const BgcMatrix2x2FP32* matrix1, const BgcMatrix2x2FP32* matrix2, BgcMatrix2x2FP32* sum);
extern inline void bgc_matrix2x2_add_fp64(const BgcMatrix2x2FP64* matrix1, const BgcMatrix2x2FP64* matrix2, BgcMatrix2x2FP64* sum);
@ -74,4 +68,3 @@ extern inline void bgc_matrix2x2_get_left_product_fp64(const BgcVector2FP64* vec
extern inline void bgc_matrix2x2_get_right_product_fp32(const BgcMatrix2x2FP32* matrix, const BgcVector2FP32* vector, BgcVector2FP32* product);
extern inline void bgc_matrix2x2_get_right_product_fp64(const BgcMatrix2x2FP64* matrix, const BgcVector2FP64* vector, BgcVector2FP64* product);

160
basic-geometry/matrix2x2.h
View file

@ -111,22 +111,22 @@ inline int bgc_matrix2x2_is_singular_fp64(const BgcMatrix2x2FP64* matrix)
// ==================== Copy ==================== //
inline void bgc_matrix2x2_copy_fp32(const BgcMatrix2x2FP32* from, BgcMatrix2x2FP32* to)
inline void bgc_matrix2x2_copy_fp32(const BgcMatrix2x2FP32* source, BgcMatrix2x2FP32* destination)
{
to->r1c1 = from->r1c1;
to->r1c2 = from->r1c2;
destination->r1c1 = source->r1c1;
destination->r1c2 = source->r1c2;
to->r2c1 = from->r2c1;
to->r2c2 = from->r2c2;
destination->r2c1 = source->r2c1;
destination->r2c2 = source->r2c2;
}
inline void bgc_matrix2x2_copy_fp64(const BgcMatrix2x2FP64* from, BgcMatrix2x2FP64* to)
inline void bgc_matrix2x2_copy_fp64(const BgcMatrix2x2FP64* source, BgcMatrix2x2FP64* destination)
{
to->r1c1 = from->r1c1;
to->r1c2 = from->r1c2;
destination->r1c1 = source->r1c1;
destination->r1c2 = source->r1c2;
to->r2c1 = from->r2c1;
to->r2c2 = from->r2c2;
destination->r2c1 = source->r2c1;
destination->r2c2 = source->r2c2;
}
// ==================== Swap ==================== //
@ -175,27 +175,27 @@ inline void bgc_matrix2x2_swap_fp64(BgcMatrix2x2FP64* matrix1, BgcMatrix2x2FP64*
// ================== Convert =================== //
inline void bgc_matrix2x2_convert_fp64_to_fp32(const BgcMatrix2x2FP64* from, BgcMatrix2x2FP32* to)
inline void bgc_matrix2x2_convert_fp64_to_fp32(const BgcMatrix2x2FP64* source, BgcMatrix2x2FP32* destination)
{
to->r1c1 = (float)from->r1c1;
to->r1c2 = (float)from->r1c2;
destination->r1c1 = (float)source->r1c1;
destination->r1c2 = (float)source->r1c2;
to->r2c1 = (float)from->r2c1;
to->r2c2 = (float)from->r2c2;
destination->r2c1 = (float)source->r2c1;
destination->r2c2 = (float)source->r2c2;
}
inline void bgc_matrix2x2_convert_fp32_to_fp64(const BgcMatrix2x2FP32* from, BgcMatrix2x2FP64* to)
inline void bgc_matrix2x2_convert_fp32_to_fp64(const BgcMatrix2x2FP32* source, BgcMatrix2x2FP64* destination)
{
to->r1c1 = from->r1c1;
to->r1c2 = from->r1c2;
destination->r1c1 = source->r1c1;
destination->r1c2 = source->r1c2;
to->r2c1 = from->r2c1;
to->r2c2 = from->r2c2;
destination->r2c1 = source->r2c1;
destination->r2c2 = source->r2c2;
}
// =================== Invert =================== //
inline int bgc_matrix2x2_invert_fp32(BgcMatrix2x2FP32* matrix)
inline int bgc_matrix2x2_invert_fp32(const BgcMatrix2x2FP32* matrix, BgcMatrix2x2FP32* inverted)
{
const float determinant = bgc_matrix2x2_get_determinant_fp32(matrix);
@ -211,16 +211,16 @@ inline int bgc_matrix2x2_invert_fp32(BgcMatrix2x2FP32* matrix)
const float multiplier = 1.0f / determinant;
matrix->r1c1 = r1c1 * multiplier;
matrix->r1c2 = r1c2 * multiplier;
inverted->r1c1 = r1c1 * multiplier;
inverted->r1c2 = r1c2 * multiplier;
matrix->r2c1 = r2c1 * multiplier;
matrix->r2c2 = r2c2 * multiplier;
inverted->r2c1 = r2c1 * multiplier;
inverted->r2c2 = r2c2 * multiplier;
return 1;
}
inline int bgc_matrix2x2_invert_fp64(BgcMatrix2x2FP64* matrix)
inline int bgc_matrix2x2_invert_fp64(const BgcMatrix2x2FP64* matrix, BgcMatrix2x2FP64* inverted)
{
const double determinant = bgc_matrix2x2_get_determinant_fp64(matrix);
@ -236,29 +236,37 @@ inline int bgc_matrix2x2_invert_fp64(BgcMatrix2x2FP64* matrix)
const double multiplier = 1.0 / determinant;
matrix->r1c1 = r1c1 * multiplier;
matrix->r1c2 = r1c2 * multiplier;
inverted->r1c1 = r1c1 * multiplier;
inverted->r1c2 = r1c2 * multiplier;
matrix->r2c1 = r2c1 * multiplier;
matrix->r2c2 = r2c2 * multiplier;
inverted->r2c1 = r2c1 * multiplier;
inverted->r2c2 = r2c2 * multiplier;
return 1;
}
// ================= Transpose ================== //
inline void bgc_matrix2x2_transpose_fp32(BgcMatrix2x2FP32* matrix)
inline void bgc_matrix2x2_transpose_fp32(const BgcMatrix2x2FP32* matrix, BgcMatrix2x2FP32* transposed)
{
const float tmp = matrix->r1c2;
matrix->r1c2 = matrix->r2c1;
matrix->r2c1 = tmp;
const float r1c2 = matrix->r1c2;
transposed->r1c1 = matrix->r1c1;
transposed->r1c2 = matrix->r2c1;
transposed->r2c1 = r1c2;
transposed->r2c2 = matrix->r2c2;
}
inline void bgc_matrix2x2_transpose_fp64(BgcMatrix2x2FP64* matrix)
inline void bgc_matrix2x2_transpose_fp64(const BgcMatrix2x2FP64* matrix, BgcMatrix2x2FP64* transposed)
{
const double tmp = matrix->r1c2;
matrix->r1c2 = matrix->r2c1;
matrix->r2c1 = tmp;
const double r1c2 = matrix->r1c2;
transposed->r1c1 = matrix->r1c1;
transposed->r1c2 = matrix->r2c1;
transposed->r2c1 = r1c2;
transposed->r2c2 = matrix->r2c2;
}
// ================= Set Row 1 ================== //
@ -317,82 +325,6 @@ inline void bgc_matrix2x2_set_column2_fp64(const double r1, const double r2, Bgc
matrix->r2c2 = r2;
}
// ================ Get Inverted ================ //
inline int bgc_matrix2x2_get_inverted_fp32(const BgcMatrix2x2FP32* matrix, BgcMatrix2x2FP32* inverted)
{
const float determinant = bgc_matrix2x2_get_determinant_fp32(matrix);
if (bgc_is_zero_fp32(determinant)) {
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;
inverted->r1c1 = r1c1 * multiplier;
inverted->r1c2 = r1c2 * multiplier;
inverted->r2c1 = r2c1 * multiplier;
inverted->r2c2 = r2c2 * multiplier;
return 1;
}
inline int bgc_matrix2x2_get_inverted_fp64(const BgcMatrix2x2FP64* matrix, BgcMatrix2x2FP64* inverted)
{
const double determinant = bgc_matrix2x2_get_determinant_fp64(matrix);
if (bgc_is_zero_fp64(determinant)) {
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;
inverted->r1c1 = r1c1 * multiplier;
inverted->r1c2 = r1c2 * multiplier;
inverted->r2c1 = r2c1 * multiplier;
inverted->r2c2 = r2c2 * multiplier;
return 1;
}
// =============== Get Transposed =============== //
inline void bgc_matrix2x2_get_transposed_fp32(const BgcMatrix2x2FP32* matrix, BgcMatrix2x2FP32* transposed)
{
float tmp = matrix->r1c2;
transposed->r1c1 = matrix->r1c1;
transposed->r1c2 = matrix->r2c1;
transposed->r2c1 = tmp;
transposed->r2c2 = matrix->r2c2;
}
inline void bgc_matrix2x2_get_transposed_fp64(const BgcMatrix2x2FP64* matrix, BgcMatrix2x2FP64* transposed)
{
double tmp = matrix->r1c2;
transposed->r1c1 = matrix->r1c1;
transposed->r1c2 = matrix->r2c1;
transposed->r2c1 = tmp;
transposed->r2c2 = matrix->r2c2;
}
// ==================== Add ===================== //
inline void bgc_matrix2x2_add_fp32(const BgcMatrix2x2FP32* matrix1, const BgcMatrix2x2FP32* matrix2, BgcMatrix2x2FP32* sum)

14
basic-geometry/matrix2x3.c
View file

@ -3,14 +3,17 @@
extern inline void bgc_matrix2x3_reset_fp32(BgcMatrix2x3FP32* matrix);
extern inline void bgc_matrix2x3_reset_fp64(BgcMatrix2x3FP64* matrix);
extern inline void bgc_matrix2x3_copy_fp32(const BgcMatrix2x3FP32* from, BgcMatrix2x3FP32* to);
extern inline void bgc_matrix2x3_copy_fp64(const BgcMatrix2x3FP64* from, BgcMatrix2x3FP64* to);
extern inline void bgc_matrix2x3_copy_fp32(const BgcMatrix2x3FP32* source, BgcMatrix2x3FP32* destination);
extern inline void bgc_matrix2x3_copy_fp64(const BgcMatrix2x3FP64* source, BgcMatrix2x3FP64* destination);
extern inline void bgc_matrix2x3_swap_fp32(BgcMatrix2x3FP32* matrix1, BgcMatrix2x3FP32* matrix2);
extern inline void bgc_matrix2x3_swap_fp64(BgcMatrix2x3FP64* matrix1, BgcMatrix2x3FP64* matrix2);
extern inline void bgc_matrix2x3_convert_fp64_to_fp32(const BgcMatrix2x3FP64* from, BgcMatrix2x3FP32* to);
extern inline void bgc_matrix2x3_convert_fp32_to_fp64(const BgcMatrix2x3FP32* from, BgcMatrix2x3FP64* to);
extern inline void bgc_matrix2x3_convert_fp64_to_fp32(const BgcMatrix2x3FP64* source, BgcMatrix2x3FP32* destination);
extern inline void bgc_matrix2x3_convert_fp32_to_fp64(const BgcMatrix2x3FP32* source, BgcMatrix2x3FP64* destination);
extern inline void bgc_matrix2x3_transpose_fp32(const BgcMatrix3x2FP32* matrix, BgcMatrix2x3FP32* transposed);
extern inline void bgc_matrix2x3_transpose_fp64(const BgcMatrix3x2FP64* matrix, BgcMatrix2x3FP64* transposed);
extern inline void bgc_matrix2x3_set_row1_fp32(const float c1, const float c2, BgcMatrix2x3FP32* matrix);
extern inline void bgc_matrix2x3_set_row1_fp64(const double c1, const double c2, BgcMatrix2x3FP64* matrix);
@ -27,9 +30,6 @@ extern inline void bgc_matrix2x3_set_column1_fp64(const double r1, const double
extern inline void bgc_matrix2x3_set_column2_fp32(const float r1, const float r2, const float r3, BgcMatrix2x3FP32* matrix);
extern inline void bgc_matrix2x3_set_column2_fp64(const double r1, const double r2, const double r3, BgcMatrix2x3FP64* matrix);
extern inline void bgc_matrix2x3_get_transposed_fp32(const BgcMatrix3x2FP32* from, BgcMatrix2x3FP32* to);
extern inline void bgc_matrix2x3_get_transposed_fp64(const BgcMatrix3x2FP64* from, BgcMatrix2x3FP64* to);
extern inline void bgc_matrix2x3_add_fp32(const BgcMatrix2x3FP32* matrix1, const BgcMatrix2x3FP32* matrix2, BgcMatrix2x3FP32* sum);
extern inline void bgc_matrix2x3_add_fp64(const BgcMatrix2x3FP64* matrix1, const BgcMatrix2x3FP64* matrix2, BgcMatrix2x3FP64* sum);

109
basic-geometry/matrix2x3.h
View file

@ -33,28 +33,28 @@ inline void bgc_matrix2x3_reset_fp64(BgcMatrix2x3FP64* matrix)
// ==================== Copy ==================== //
inline void bgc_matrix2x3_copy_fp32(const BgcMatrix2x3FP32* from, BgcMatrix2x3FP32* to)
inline void bgc_matrix2x3_copy_fp32(const BgcMatrix2x3FP32* source, BgcMatrix2x3FP32* destination)
{
to->r1c1 = from->r1c1;
to->r1c2 = from->r1c2;
destination->r1c1 = source->r1c1;
destination->r1c2 = source->r1c2;
to->r2c1 = from->r2c1;
to->r2c2 = from->r2c2;
destination->r2c1 = source->r2c1;
destination->r2c2 = source->r2c2;
to->r3c1 = from->r3c1;
to->r3c2 = from->r3c2;
destination->r3c1 = source->r3c1;
destination->r3c2 = source->r3c2;
}
inline void bgc_matrix2x3_copy_fp64(const BgcMatrix2x3FP64* from, BgcMatrix2x3FP64* to)
inline void bgc_matrix2x3_copy_fp64(const BgcMatrix2x3FP64* source, BgcMatrix2x3FP64* destination)
{
to->r1c1 = from->r1c1;
to->r1c2 = from->r1c2;
destination->r1c1 = source->r1c1;
destination->r1c2 = source->r1c2;
to->r2c1 = from->r2c1;
to->r2c2 = from->r2c2;
destination->r2c1 = source->r2c1;
destination->r2c2 = source->r2c2;
to->r3c1 = from->r3c1;
to->r3c2 = from->r3c2;
destination->r3c1 = source->r3c1;
destination->r3c2 = source->r3c2;
}
// ==================== Swap ==================== //
@ -121,28 +121,54 @@ inline void bgc_matrix2x3_swap_fp64(BgcMatrix2x3FP64* matrix1, BgcMatrix2x3FP64*
// ================== Convert =================== //
inline void bgc_matrix2x3_convert_fp64_to_fp32(const BgcMatrix2x3FP64* from, BgcMatrix2x3FP32* to)
inline void bgc_matrix2x3_convert_fp64_to_fp32(const BgcMatrix2x3FP64* source, BgcMatrix2x3FP32* destination)
{
to->r1c1 = (float) from->r1c1;
to->r1c2 = (float) from->r1c2;
destination->r1c1 = (float)source->r1c1;
destination->r1c2 = (float)source->r1c2;
to->r2c1 = (float) from->r2c1;
to->r2c2 = (float) from->r2c2;
destination->r2c1 = (float)source->r2c1;
destination->r2c2 = (float)source->r2c2;
to->r3c1 = (float) from->r3c1;
to->r3c2 = (float) from->r3c2;
destination->r3c1 = (float)source->r3c1;
destination->r3c2 = (float)source->r3c2;
}
inline void bgc_matrix2x3_convert_fp32_to_fp64(const BgcMatrix2x3FP32* from, BgcMatrix2x3FP64* to)
inline void bgc_matrix2x3_convert_fp32_to_fp64(const BgcMatrix2x3FP32* source, BgcMatrix2x3FP64* destination)
{
to->r1c1 = from->r1c1;
to->r1c2 = from->r1c2;
destination->r1c1 = source->r1c1;
destination->r1c2 = source->r1c2;
to->r2c1 = from->r2c1;
to->r2c2 = from->r2c2;
destination->r2c1 = source->r2c1;
destination->r2c2 = source->r2c2;
to->r3c1 = from->r3c1;
to->r3c2 = from->r3c2;
destination->r3c1 = source->r3c1;
destination->r3c2 = source->r3c2;
}
// ================= Transpose ================== //
inline void bgc_matrix2x3_transpose_fp32(const BgcMatrix3x2FP32* matrix, BgcMatrix2x3FP32* transposed)
{
transposed->r1c1 = matrix->r1c1;
transposed->r1c2 = matrix->r2c1;
transposed->r2c1 = matrix->r1c2;
transposed->r2c2 = matrix->r2c2;
transposed->r3c1 = matrix->r1c3;
transposed->r3c2 = matrix->r2c3;
}
inline void bgc_matrix2x3_transpose_fp64(const BgcMatrix3x2FP64* matrix, BgcMatrix2x3FP64* transposed)
{
transposed->r1c1 = matrix->r1c1;
transposed->r1c2 = matrix->r2c1;
transposed->r2c1 = matrix->r1c2;
transposed->r2c2 = matrix->r2c2;
transposed->r3c1 = matrix->r1c3;
transposed->r3c2 = matrix->r2c3;
}
// ================= Set Row 1 ================== //
@ -219,33 +245,6 @@ inline void bgc_matrix2x3_set_column2_fp64(const double r1, const double r2, con
matrix->r3c2 = r3;
}
// =============== Get transposed =============== //
inline void bgc_matrix2x3_get_transposed_fp32(const BgcMatrix3x2FP32* matrix, BgcMatrix2x3FP32* transposed)
{
transposed->r1c1 = matrix->r1c1;
transposed->r1c2 = matrix->r2c1;
transposed->r2c1 = matrix->r1c2;
transposed->r2c2 = matrix->r2c2;
transposed->r3c1 = matrix->r1c3;
transposed->r3c2 = matrix->r2c3;
}
inline void bgc_matrix2x3_get_transposed_fp64(const BgcMatrix3x2FP64* matrix, BgcMatrix2x3FP64* transposed)
{
transposed->r1c1 = matrix->r1c1;
transposed->r1c2 = matrix->r2c1;
transposed->r2c1 = matrix->r1c2;
transposed->r2c2 = matrix->r2c2;
transposed->r3c1 = matrix->r1c3;
transposed->r3c2 = matrix->r2c3;
}
// ==================== Add ===================== //
inline void bgc_matrix2x3_add_fp32(const BgcMatrix2x3FP32* matrix1, const BgcMatrix2x3FP32* matrix2, BgcMatrix2x3FP32* sum)

14
basic-geometry/matrix3x2.c
View file

@ -3,14 +3,17 @@
extern inline void bgc_matrix3x2_reset_fp32(BgcMatrix3x2FP32* matrix);
extern inline void bgc_matrix3x2_reset_fp64(BgcMatrix3x2FP64* matrix);
extern inline void bgc_matrix3x2_copy_fp32(const BgcMatrix3x2FP32* from, BgcMatrix3x2FP32* to);
extern inline void bgc_matrix3x2_copy_fp64(const BgcMatrix3x2FP64* from, BgcMatrix3x2FP64* to);
extern inline void bgc_matrix3x2_copy_fp32(const BgcMatrix3x2FP32* source, BgcMatrix3x2FP32* destination);
extern inline void bgc_matrix3x2_copy_fp64(const BgcMatrix3x2FP64* source, BgcMatrix3x2FP64* destination);
extern inline void bgc_matrix3x2_swap_fp32(BgcMatrix3x2FP32* matrix1, BgcMatrix3x2FP32* matrix2);
extern inline void bgc_matrix3x2_swap_fp64(BgcMatrix3x2FP64* matrix1, BgcMatrix3x2FP64* matrix2);
extern inline void bgc_matrix3x2_convert_fp64_to_fp32(const BgcMatrix3x2FP64* from, BgcMatrix3x2FP32* to);
extern inline void bgc_matrix3x2_convert_fp32_to_fp64(const BgcMatrix3x2FP32* from, BgcMatrix3x2FP64* to);
extern inline void bgc_matrix3x2_convert_fp64_to_fp32(const BgcMatrix3x2FP64* source, BgcMatrix3x2FP32* destination);
extern inline void bgc_matrix3x2_convert_fp32_to_fp64(const BgcMatrix3x2FP32* source, BgcMatrix3x2FP64* destination);
extern inline void bgc_matrix3x2_transpose_fp32(const BgcMatrix2x3FP32* matrix, BgcMatrix3x2FP32* transposed);
extern inline void bgc_matrix3x2_transpose_fp64(const BgcMatrix2x3FP64* matrix, BgcMatrix3x2FP64* transposed);
extern inline void bgc_matrix3x2_set_row1_fp32(const float c1, const float c2, const float c3, BgcMatrix3x2FP32* matrix);
extern inline void bgc_matrix3x2_set_row1_fp64(const double c1, const double c2, const double c3, BgcMatrix3x2FP64* matrix);
@ -27,9 +30,6 @@ extern inline void bgc_matrix3x2_set_column2_fp64(const double r1, const double
extern inline void bgc_matrix3x2_set_column3_fp32(const float r1, const float r2, BgcMatrix3x2FP32* matrix);
extern inline void bgc_matrix3x2_set_column3_fp64(const double r1, const double r2, BgcMatrix3x2FP64* matrix);
extern inline void bgc_matrix3x2_get_transposed_fp32(const BgcMatrix2x3FP32* from, BgcMatrix3x2FP32* to);
extern inline void bgc_matrix3x2_get_transposed_fp64(const BgcMatrix2x3FP64* from, BgcMatrix3x2FP64* to);
extern inline void bgc_matrix3x2_add_fp32(const BgcMatrix3x2FP32* matrix1, const BgcMatrix3x2FP32* matrix2, BgcMatrix3x2FP32* sum);
extern inline void bgc_matrix3x2_add_fp64(const BgcMatrix3x2FP64* matrix1, const BgcMatrix3x2FP64* matrix2, BgcMatrix3x2FP64* sum);

104
basic-geometry/matrix3x2.h
View file

@ -31,26 +31,26 @@ inline void bgc_matrix3x2_reset_fp64(BgcMatrix3x2FP64* matrix)
// ==================== Copy ==================== //
inline void bgc_matrix3x2_copy_fp32(const BgcMatrix3x2FP32* from, BgcMatrix3x2FP32* to)
inline void bgc_matrix3x2_copy_fp32(const BgcMatrix3x2FP32* source, BgcMatrix3x2FP32* destination)
{
to->r1c1 = from->r1c1;
to->r1c2 = from->r1c2;
to->r1c3 = from->r1c3;
destination->r1c1 = source->r1c1;
destination->r1c2 = source->r1c2;
destination->r1c3 = source->r1c3;
to->r2c1 = from->r2c1;
to->r2c2 = from->r2c2;
to->r2c3 = from->r2c3;
destination->r2c1 = source->r2c1;
destination->r2c2 = source->r2c2;
destination->r2c3 = source->r2c3;
}
inline void bgc_matrix3x2_copy_fp64(const BgcMatrix3x2FP64* from, BgcMatrix3x2FP64* to)
inline void bgc_matrix3x2_copy_fp64(const BgcMatrix3x2FP64* source, BgcMatrix3x2FP64* destination)
{
to->r1c1 = from->r1c1;
to->r1c2 = from->r1c2;
to->r1c3 = from->r1c3;
destination->r1c1 = source->r1c1;
destination->r1c2 = source->r1c2;
destination->r1c3 = source->r1c3;
to->r2c1 = from->r2c1;
to->r2c2 = from->r2c2;
to->r2c3 = from->r2c3;
destination->r2c1 = source->r2c1;
destination->r2c2 = source->r2c2;
destination->r2c3 = source->r2c3;
}
// ==================== Swap ==================== //
@ -111,26 +111,50 @@ inline void bgc_matrix3x2_swap_fp64(BgcMatrix3x2FP64* matrix1, BgcMatrix3x2FP64*
// ================== Convert =================== //
inline void bgc_matrix3x2_convert_fp64_to_fp32(const BgcMatrix3x2FP64* from, BgcMatrix3x2FP32* to)
inline void bgc_matrix3x2_convert_fp64_to_fp32(const BgcMatrix3x2FP64* source, BgcMatrix3x2FP32* destination)
{
to->r1c1 = (float) from->r1c1;
to->r1c2 = (float) from->r1c2;
to->r1c3 = (float) from->r1c3;
destination->r1c1 = (float)source->r1c1;
destination->r1c2 = (float)source->r1c2;
destination->r1c3 = (float)source->r1c3;
to->r2c1 = (float) from->r2c1;
to->r2c2 = (float) from->r2c2;
to->r2c3 = (float) from->r2c3;
destination->r2c1 = (float)source->r2c1;
destination->r2c2 = (float)source->r2c2;
destination->r2c3 = (float)source->r2c3;
}
inline void bgc_matrix3x2_convert_fp32_to_fp64(const BgcMatrix3x2FP32* from, BgcMatrix3x2FP64* to)
inline void bgc_matrix3x2_convert_fp32_to_fp64(const BgcMatrix3x2FP32* source, BgcMatrix3x2FP64* destination)
{
to->r1c1 = from->r1c1;
to->r1c2 = from->r1c2;
to->r1c3 = from->r1c3;
destination->r1c1 = source->r1c1;
destination->r1c2 = source->r1c2;
destination->r1c3 = source->r1c3;
to->r2c1 = from->r2c1;
to->r2c2 = from->r2c2;
to->r2c3 = from->r2c3;
destination->r2c1 = source->r2c1;
destination->r2c2 = source->r2c2;
destination->r2c3 = source->r2c3;
}
// ================= Transpose ================== //
inline void bgc_matrix3x2_transpose_fp32(const BgcMatrix2x3FP32* matrix, BgcMatrix3x2FP32* transposed)
{
transposed->r1c1 = matrix->r1c1;
transposed->r1c2 = matrix->r2c1;
transposed->r1c3 = matrix->r3c1;
transposed->r2c1 = matrix->r1c2;
transposed->r2c2 = matrix->r2c2;
transposed->r2c3 = matrix->r3c2;
}
inline void bgc_matrix3x2_transpose_fp64(const BgcMatrix2x3FP64* matrix, BgcMatrix3x2FP64* transposed)
{
transposed->r1c1 = matrix->r1c1;
transposed->r1c2 = matrix->r2c1;
transposed->r1c3 = matrix->r3c1;
transposed->r2c1 = matrix->r1c2;
transposed->r2c2 = matrix->r2c2;
transposed->r2c3 = matrix->r3c2;
}
// ================= Set Row 1 ================== //
@ -207,30 +231,6 @@ inline void bgc_matrix3x2_set_column3_fp64(const double r1, const double r2, Bgc
matrix->r2c3 = r2;
}
// =============== Get transposed =============== //
inline void bgc_matrix3x2_get_transposed_fp32(const BgcMatrix2x3FP32* from, BgcMatrix3x2FP32* to)
{
to->r1c1 = from->r1c1;
to->r1c2 = from->r2c1;
to->r1c3 = from->r3c1;
to->r2c1 = from->r1c2;
to->r2c2 = from->r2c2;
to->r2c3 = from->r3c2;
}
inline void bgc_matrix3x2_get_transposed_fp64(const BgcMatrix2x3FP64* from, BgcMatrix3x2FP64* to)
{
to->r1c1 = from->r1c1;
to->r1c2 = from->r2c1;
to->r1c3 = from->r3c1;
to->r2c1 = from->r1c2;
to->r2c2 = from->r2c2;
to->r2c3 = from->r3c2;
}
// ==================== Add ===================== //
inline void bgc_matrix3x2_add_fp32(const BgcMatrix3x2FP32* matrix1, const BgcMatrix3x2FP32* matrix2, BgcMatrix3x2FP32* sum)

135
basic-geometry/matrix3x3.c
View file

@ -9,14 +9,14 @@ extern inline void bgc_matrix3x3_set_to_identity_fp64(BgcMatrix3x3FP64* matrix);
extern inline void bgc_matrix3x3_set_to_diagonal_fp32(const float d1, const float d2, const float d3, BgcMatrix3x3FP32* matrix);
extern inline void bgc_matrix3x3_set_to_diagonal_fp64(const double d1, const double d2, const double d3, BgcMatrix3x3FP64* matrix);
extern inline void bgc_matrix3x3_copy_fp32(const BgcMatrix3x3FP32* from, BgcMatrix3x3FP32* to);
extern inline void bgc_matrix3x3_copy_fp64(const BgcMatrix3x3FP64* from, BgcMatrix3x3FP64* to);
extern inline void bgc_matrix3x3_copy_fp32(const BgcMatrix3x3FP32* source, BgcMatrix3x3FP32* destination);
extern inline void bgc_matrix3x3_copy_fp64(const BgcMatrix3x3FP64* source, BgcMatrix3x3FP64* destination);
extern inline void bgc_matrix3x3_swap_fp32(BgcMatrix3x3FP32* matrix1, BgcMatrix3x3FP32* matrix2);
extern inline void bgc_matrix3x3_swap_fp64(BgcMatrix3x3FP64* matrix1, BgcMatrix3x3FP64* matrix2);
extern inline void bgc_matrix3x3_convert_fp64_to_fp32(const BgcMatrix3x3FP64* from, BgcMatrix3x3FP32* to);
extern inline void bgc_matrix3x3_convert_fp32_to_fp64(const BgcMatrix3x3FP32* from, BgcMatrix3x3FP64* to);
extern inline void bgc_matrix3x3_convert_fp64_to_fp32(const BgcMatrix3x3FP64* source, BgcMatrix3x3FP32* destination);
extern inline void bgc_matrix3x3_convert_fp32_to_fp64(const BgcMatrix3x3FP32* source, BgcMatrix3x3FP64* destination);
extern inline float bgc_matrix3x3_get_determinant_fp32(const BgcMatrix3x3FP32* matrix);
extern inline double bgc_matrix3x3_get_determinant_fp64(const BgcMatrix3x3FP64* matrix);
@ -24,8 +24,8 @@ extern inline double bgc_matrix3x3_get_determinant_fp64(const BgcMatrix3x3FP64*
extern inline int bgc_matrix3x3_is_singular_fp32(const BgcMatrix3x3FP32* matrix);
extern inline int bgc_matrix3x3_is_singular_fp64(const BgcMatrix3x3FP64* matrix);
extern inline void bgc_matrix3x3_transpose_fp32(BgcMatrix3x3FP32* matrix);
extern inline void bgc_matrix3x3_transpose_fp64(BgcMatrix3x3FP64* matrix);
extern inline void bgc_matrix3x3_transpose_fp32(const BgcMatrix3x3FP32* matrix, BgcMatrix3x3FP32* transposed);
extern inline void bgc_matrix3x3_transpose_fp64(const BgcMatrix3x3FP64* matrix, BgcMatrix3x3FP64* transposed);
extern inline void bgc_matrix3x3_set_row1_fp32(const float c1, const float c2, const float c3, BgcMatrix3x3FP32* matrix);
extern inline void bgc_matrix3x3_set_row1_fp64(const double c1, const double c2, const double c3, BgcMatrix3x3FP64* matrix);
@ -45,9 +45,6 @@ extern inline void bgc_matrix3x3_set_column2_fp64(const double r1, const double
extern inline void bgc_matrix3x3_set_column3_fp32(const float r1, const float r2, const float r3, BgcMatrix3x3FP32* matrix);
extern inline void bgc_matrix3x3_set_column3_fp64(const double r1, const double r2, const double r3, BgcMatrix3x3FP64* matrix);
extern inline void bgc_matrix3x3_get_transposed_fp32(const BgcMatrix3x3FP32* matrix, BgcMatrix3x3FP32* result);
extern inline void bgc_matrix3x3_get_transposed_fp64(const BgcMatrix3x3FP64* matrix, BgcMatrix3x3FP64* result);
extern inline void bgc_matrix3x3_add_fp32(const BgcMatrix3x3FP32* matrix1, const BgcMatrix3x3FP32* matrix2, BgcMatrix3x3FP32* sum);
extern inline void bgc_matrix3x3_add_fp64(const BgcMatrix3x3FP64* matrix1, const BgcMatrix3x3FP64* matrix2, BgcMatrix3x3FP64* sum);
@ -74,11 +71,11 @@ extern inline void bgc_matrix3x3_get_right_product_fp64(const BgcMatrix3x3FP64*
// =================== Invert =================== //
int bgc_matrix3x3_invert_fp32(BgcMatrix3x3FP32* matrix)
int bgc_matrix3x3_invert_fp32(const BgcMatrix3x3FP32* matrix, BgcMatrix3x3FP32* inverted)
{
const float determinant = bgc_matrix3x3_get_determinant_fp32(matrix);
if (-BGC_EPSYLON_FP32 <= determinant && determinant <= BGC_EPSYLON_FP32) {
if (bgc_is_zero_fp32(determinant)) {
return 0;
}
@ -96,26 +93,26 @@ int bgc_matrix3x3_invert_fp32(BgcMatrix3x3FP32* matrix)
const float multiplier = 1.0f / determinant;
matrix->r1c1 = r1c1 * multiplier;
matrix->r1c2 = r1c2 * multiplier;
matrix->r1c3 = r1c3 * multiplier;
inverted->r1c1 = r1c1 * multiplier;
inverted->r1c2 = r1c2 * multiplier;
inverted->r1c3 = r1c3 * multiplier;
matrix->r2c1 = r2c1 * multiplier;
matrix->r2c2 = r2c2 * multiplier;
matrix->r2c3 = r2c3 * multiplier;
inverted->r2c1 = r2c1 * multiplier;
inverted->r2c2 = r2c2 * multiplier;
inverted->r2c3 = r2c3 * multiplier;
matrix->r3c1 = r3c1 * multiplier;
matrix->r3c2 = r3c2 * multiplier;
matrix->r3c3 = r3c3 * multiplier;
inverted->r3c1 = r3c1 * multiplier;
inverted->r3c2 = r3c2 * multiplier;
inverted->r3c3 = r3c3 * multiplier;
return 1;
}
int bgc_matrix3x3_invert_fp64(BgcMatrix3x3FP64* matrix)
int bgc_matrix3x3_invert_fp64(const BgcMatrix3x3FP64* matrix, BgcMatrix3x3FP64* inverted)
{
const double determinant = bgc_matrix3x3_get_determinant_fp64(matrix);
if (-BGC_EPSYLON_FP64 <= determinant && determinant <= BGC_EPSYLON_FP64) {
if (bgc_is_zero_fp64(determinant)) {
return 0;
}
@ -133,93 +130,17 @@ int bgc_matrix3x3_invert_fp64(BgcMatrix3x3FP64* matrix)
const double multiplier = 1.0 / determinant;
matrix->r1c1 = r1c1 * multiplier;
matrix->r1c2 = r1c2 * multiplier;
matrix->r1c3 = r1c3 * multiplier;
inverted->r1c1 = r1c1 * multiplier;
inverted->r1c2 = r1c2 * multiplier;
inverted->r1c3 = r1c3 * multiplier;
matrix->r2c1 = r2c1 * multiplier;
matrix->r2c2 = r2c2 * multiplier;
matrix->r2c3 = r2c3 * multiplier;
inverted->r2c1 = r2c1 * multiplier;
inverted->r2c2 = r2c2 * multiplier;
inverted->r2c3 = r2c3 * multiplier;
matrix->r3c1 = r3c1 * multiplier;
matrix->r3c2 = r3c2 * multiplier;
matrix->r3c3 = r3c3 * multiplier;
return 1;
}
// ================ Get Inverted ================ //
int bgc_matrix3x3_get_inverted_fp32(const BgcMatrix3x3FP32* matrix, BgcMatrix3x3FP32* result)
{
const float determinant = bgc_matrix3x3_get_determinant_fp32(matrix);
if (-BGC_EPSYLON_FP32 <= determinant && determinant <= BGC_EPSYLON_FP32) {
return 0;
}
const float r1c1 = matrix->r2c2 * matrix->r3c3 - matrix->r2c3 * matrix->r3c2;
const float r1c2 = matrix->r1c3 * matrix->r3c2 - matrix->r1c2 * matrix->r3c3;
const float r1c3 = matrix->r1c2 * matrix->r2c3 - matrix->r1c3 * matrix->r2c2;
const float r2c1 = matrix->r2c3 * matrix->r3c1 - matrix->r2c1 * matrix->r3c3;
const float r2c2 = matrix->r1c1 * matrix->r3c3 - matrix->r1c3 * matrix->r3c1;
const float r2c3 = matrix->r1c3 * matrix->r2c1 - matrix->r1c1 * matrix->r2c3;
const float r3c1 = matrix->r2c1 * matrix->r3c2 - matrix->r2c2 * matrix->r3c1;
const float r3c2 = matrix->r1c2 * matrix->r3c1 - matrix->r1c1 * matrix->r3c2;
const float r3c3 = matrix->r1c1 * matrix->r2c2 - matrix->r1c2 * matrix->r2c1;
const float multiplier = 1.0f / determinant;
result->r1c1 = r1c1 * multiplier;
result->r1c2 = r1c2 * multiplier;
result->r1c3 = r1c3 * multiplier;
result->r2c1 = r2c1 * multiplier;
result->r2c2 = r2c2 * multiplier;
result->r2c3 = r2c3 * multiplier;
result->r3c1 = r3c1 * multiplier;
result->r3c2 = r3c2 * multiplier;
result->r3c3 = r3c3 * multiplier;
return 1;
}
int bgc_matrix3x3_get_inverted_fp64(const BgcMatrix3x3FP64* matrix, BgcMatrix3x3FP64* result)
{
const double determinant = bgc_matrix3x3_get_determinant_fp64(matrix);
if (-BGC_EPSYLON_FP64 <= determinant && determinant <= BGC_EPSYLON_FP64) {
return 0;
}
const double r1c1 = matrix->r2c2 * matrix->r3c3 - matrix->r2c3 * matrix->r3c2;
const double r1c2 = matrix->r1c3 * matrix->r3c2 - matrix->r1c2 * matrix->r3c3;
const double r1c3 = matrix->r1c2 * matrix->r2c3 - matrix->r1c3 * matrix->r2c2;
const double r2c1 = matrix->r2c3 * matrix->r3c1 - matrix->r2c1 * matrix->r3c3;
const double r2c2 = matrix->r1c1 * matrix->r3c3 - matrix->r1c3 * matrix->r3c1;
const double r2c3 = matrix->r1c3 * matrix->r2c1 - matrix->r1c1 * matrix->r2c3;
const double r3c1 = matrix->r2c1 * matrix->r3c2 - matrix->r2c2 * matrix->r3c1;
const double r3c2 = matrix->r1c2 * matrix->r3c1 - matrix->r1c1 * matrix->r3c2;
const double r3c3 = matrix->r1c1 * matrix->r2c2 - matrix->r1c2 * matrix->r2c1;
const double multiplier = 1.0 / determinant;
result->r1c1 = r1c1 * multiplier;
result->r1c2 = r1c2 * multiplier;
result->r1c3 = r1c3 * multiplier;
result->r2c1 = r2c1 * multiplier;
result->r2c2 = r2c2 * multiplier;
result->r2c3 = r2c3 * multiplier;
result->r3c1 = r3c1 * multiplier;
result->r3c2 = r3c2 * multiplier;
result->r3c3 = r3c3 * multiplier;
inverted->r3c1 = r3c1 * multiplier;
inverted->r3c2 = r3c2 * multiplier;
inverted->r3c3 = r3c3 * multiplier;
return 1;
}

180
basic-geometry/matrix3x3.h
View file

@ -102,34 +102,34 @@ inline void bgc_matrix3x3_set_to_diagonal_fp64(const double d1, const double d2,
// ==================== Copy ==================== //
inline void bgc_matrix3x3_copy_fp32(const BgcMatrix3x3FP32* from, BgcMatrix3x3FP32* to)
inline void bgc_matrix3x3_copy_fp32(const BgcMatrix3x3FP32* source, BgcMatrix3x3FP32* destination)
{
to->r1c1 = from->r1c1;
to->r1c2 = from->r1c2;
to->r1c3 = from->r1c3;
destination->r1c1 = source->r1c1;
destination->r1c2 = source->r1c2;
destination->r1c3 = source->r1c3;
to->r2c1 = from->r2c1;
to->r2c2 = from->r2c2;
to->r2c3 = from->r2c3;
destination->r2c1 = source->r2c1;
destination->r2c2 = source->r2c2;
destination->r2c3 = source->r2c3;
to->r3c1 = from->r3c1;
to->r3c2 = from->r3c2;
to->r3c3 = from->r3c3;
destination->r3c1 = source->r3c1;
destination->r3c2 = source->r3c2;
destination->r3c3 = source->r3c3;
}
inline void bgc_matrix3x3_copy_fp64(const BgcMatrix3x3FP64* from, BgcMatrix3x3FP64* to)
inline void bgc_matrix3x3_copy_fp64(const BgcMatrix3x3FP64* source, BgcMatrix3x3FP64* destination)
{
to->r1c1 = from->r1c1;
to->r1c2 = from->r1c2;
to->r1c3 = from->r1c3;
destination->r1c1 = source->r1c1;
destination->r1c2 = source->r1c2;
destination->r1c3 = source->r1c3;
to->r2c1 = from->r2c1;
to->r2c2 = from->r2c2;
to->r2c3 = from->r2c3;
destination->r2c1 = source->r2c1;
destination->r2c2 = source->r2c2;
destination->r2c3 = source->r2c3;
to->r3c1 = from->r3c1;
to->r3c2 = from->r3c2;
to->r3c3 = from->r3c3;
destination->r3c1 = source->r3c1;
destination->r3c2 = source->r3c2;
destination->r3c3 = source->r3c3;
}
// ==================== Swap ==================== //
@ -214,34 +214,34 @@ inline void bgc_matrix3x3_swap_fp64(BgcMatrix3x3FP64* matrix1, BgcMatrix3x3FP64*
// ================== Convert =================== //
inline void bgc_matrix3x3_convert_fp64_to_fp32(const BgcMatrix3x3FP64* from, BgcMatrix3x3FP32* to)
inline void bgc_matrix3x3_convert_fp64_to_fp32(const BgcMatrix3x3FP64* source, BgcMatrix3x3FP32* destination)
{
to->r1c1 = (float) from->r1c1;
to->r1c2 = (float) from->r1c2;
to->r1c3 = (float) from->r1c3;
destination->r1c1 = (float)source->r1c1;
destination->r1c2 = (float)source->r1c2;
destination->r1c3 = (float)source->r1c3;
to->r2c1 = (float) from->r2c1;
to->r2c2 = (float) from->r2c2;
to->r2c3 = (float) from->r2c3;
destination->r2c1 = (float)source->r2c1;
destination->r2c2 = (float)source->r2c2;
destination->r2c3 = (float)source->r2c3;
to->r3c1 = (float) from->r3c1;
to->r3c2 = (float) from->r3c2;
to->r3c3 = (float) from->r3c3;
destination->r3c1 = (float)source->r3c1;
destination->r3c2 = (float)source->r3c2;
destination->r3c3 = (float)source->r3c3;
}
inline void bgc_matrix3x3_convert_fp32_to_fp64(const BgcMatrix3x3FP32* from, BgcMatrix3x3FP64* to)
inline void bgc_matrix3x3_convert_fp32_to_fp64(const BgcMatrix3x3FP32* source, BgcMatrix3x3FP64* destination)
{
to->r1c1 = from->r1c1;
to->r1c2 = from->r1c2;
to->r1c3 = from->r1c3;
destination->r1c1 = source->r1c1;
destination->r1c2 = source->r1c2;
destination->r1c3 = source->r1c3;
to->r2c1 = from->r2c1;
to->r2c2 = from->r2c2;
to->r2c3 = from->r2c3;
destination->r2c1 = source->r2c1;
destination->r2c2 = source->r2c2;
destination->r2c3 = source->r2c3;
to->r3c1 = from->r3c1;
to->r3c2 = from->r3c2;
to->r3c3 = from->r3c3;
destination->r3c1 = source->r3c1;
destination->r3c2 = source->r3c2;
destination->r3c3 = source->r3c3;
}
// ================ Determinant ================= //
@ -274,40 +274,48 @@ inline int bgc_matrix3x3_is_singular_fp64(const BgcMatrix3x3FP64* matrix)
// =================== Invert =================== //
int bgc_matrix3x3_invert_fp32(BgcMatrix3x3FP32* matrix);
int bgc_matrix3x3_invert_fp32(const BgcMatrix3x3FP32* matrix, BgcMatrix3x3FP32* inverted);
int bgc_matrix3x3_invert_fp64(BgcMatrix3x3FP64* matrix);
int bgc_matrix3x3_invert_fp64(const BgcMatrix3x3FP64* matrix, BgcMatrix3x3FP64* inverted);
// ================= Transpose ================== //
inline void bgc_matrix3x3_transpose_fp32(BgcMatrix3x3FP32* matrix)
inline void bgc_matrix3x3_transpose_fp32(const BgcMatrix3x3FP32* matrix, BgcMatrix3x3FP32* transposed)
{
float tmp = matrix->r1c2;
matrix->r1c2 = matrix->r2c1;
matrix->r2c1 = tmp;
transposed->r1c1 = matrix->r1c1;
transposed->r2c2 = matrix->r2c2;
transposed->r3c3 = matrix->r3c3;
tmp = matrix->r1c3;
matrix->r1c3 = matrix->r3c1;
matrix->r3c1 = tmp;
const float r1c2 = matrix->r1c2;
const float r1c3 = matrix->r1c3;
const float r2c3 = matrix->r2c3;
tmp = matrix->r2c3;
matrix->r2c3 = matrix->r3c2;
matrix->r3c2 = tmp;
transposed->r1c2 = matrix->r2c1;
transposed->r1c3 = matrix->r3c1;
transposed->r2c3 = matrix->r3c2;
transposed->r2c1 = r1c2;
transposed->r3c1 = r1c3;
transposed->r3c2 = r2c3;
}
inline void bgc_matrix3x3_transpose_fp64(BgcMatrix3x3FP64* matrix)
inline void bgc_matrix3x3_transpose_fp64(const BgcMatrix3x3FP64* matrix, BgcMatrix3x3FP64* transposed)
{
double tmp = matrix->r1c2;
matrix->r1c2 = matrix->r2c1;
matrix->r2c1 = tmp;
transposed->r1c1 = matrix->r1c1;
transposed->r2c2 = matrix->r2c2;
transposed->r3c3 = matrix->r3c3;
tmp = matrix->r1c3;
matrix->r1c3 = matrix->r3c1;
matrix->r3c1 = tmp;
const double r1c2 = matrix->r1c2;
const double r1c3 = matrix->r1c3;
const double r2c3 = matrix->r2c3;
tmp = matrix->r2c3;
matrix->r2c3 = matrix->r3c2;
matrix->r3c2 = tmp;
transposed->r1c2 = matrix->r2c1;
transposed->r1c3 = matrix->r3c1;
transposed->r2c3 = matrix->r3c2;
transposed->r2c1 = r1c2;
transposed->r3c1 = r1c3;
transposed->r3c2 = r2c3;
}
// ================= Set Row 1 ================== //
@ -406,54 +414,6 @@ inline void bgc_matrix3x3_set_column3_fp64(const double r1, const double r2, con
matrix->r3c3 = r3;
}
// ================ Get Inverted ================ //
int bgc_matrix3x3_get_inverted_fp32(const BgcMatrix3x3FP32* matrix, BgcMatrix3x3FP32* result);
int bgc_matrix3x3_get_inverted_fp64(const BgcMatrix3x3FP64* matrix, BgcMatrix3x3FP64* result);
// =============== Get Transposed =============== //
inline void bgc_matrix3x3_get_transposed_fp32(const BgcMatrix3x3FP32* matrix, BgcMatrix3x3FP32* result)
{
if (matrix == result) {
bgc_matrix3x3_transpose_fp32(result);
return;
}
result->r1c1 = matrix->r1c1;
result->r1c2 = matrix->r2c1;
result->r1c3 = matrix->r3c1;
result->r2c1 = matrix->r1c2;
result->r2c2 = matrix->r2c2;
result->r2c3 = matrix->r3c2;
result->r3c1 = matrix->r1c3;
result->r3c2 = matrix->r2c3;
result->r3c3 = matrix->r3c3;
}
inline void bgc_matrix3x3_get_transposed_fp64(const BgcMatrix3x3FP64* matrix, BgcMatrix3x3FP64* result)
{
if (matrix == result) {
bgc_matrix3x3_transpose_fp64(result);
return;
}
result->r1c1 = matrix->r1c1;
result->r1c2 = matrix->r2c1;
result->r1c3 = matrix->r3c1;
result->r2c1 = matrix->r1c2;
result->r2c2 = matrix->r2c2;
result->r2c3 = matrix->r3c2;
result->r3c1 = matrix->r1c3;
result->r3c2 = matrix->r2c3;
result->r3c3 = matrix->r3c3;
}
// ==================== Add ===================== //
inline void bgc_matrix3x3_add_fp32(const BgcMatrix3x3FP32* matrix1, const BgcMatrix3x3FP32* matrix2, BgcMatrix3x3FP32* sum)

36
basic-geometry/quaternion.c
View file

@ -21,35 +21,29 @@ extern inline int bgc_quaternion_is_zero_fp64(const BgcQuaternionFP64* quaternio
extern inline int bgc_quaternion_is_unit_fp32(const BgcQuaternionFP32* quaternion);
extern inline int bgc_quaternion_is_unit_fp64(const BgcQuaternionFP64* quaternion);
extern inline void bgc_quaternion_copy_fp32(const BgcQuaternionFP32* from, BgcQuaternionFP32* to);
extern inline void bgc_quaternion_copy_fp64(const BgcQuaternionFP64* from, BgcQuaternionFP64* to);
extern inline void bgc_quaternion_copy_fp32(const BgcQuaternionFP32* source, BgcQuaternionFP32* destination);
extern inline void bgc_quaternion_copy_fp64(const BgcQuaternionFP64* source, BgcQuaternionFP64* destination);
extern inline void bgc_quaternion_swap_fp32(BgcQuaternionFP32* quarternion1, BgcQuaternionFP32* quarternion2);
extern inline void bgc_quaternion_swap_fp64(BgcQuaternionFP64* quarternion1, BgcQuaternionFP64* quarternion2);
extern inline void bgc_quaternion_convert_fp64_to_fp32(const BgcQuaternionFP64* quaternion, BgcQuaternionFP32* result);
extern inline void bgc_quaternion_convert_fp32_to_fp64(const BgcQuaternionFP32* quaternion, BgcQuaternionFP64* result);
extern inline void bgc_quaternion_convert_fp64_to_fp32(const BgcQuaternionFP64* source, BgcQuaternionFP32* destination);
extern inline void bgc_quaternion_convert_fp32_to_fp64(const BgcQuaternionFP32* source, BgcQuaternionFP64* destination);
extern inline void bgc_quaternion_conjugate_fp32(BgcQuaternionFP32* quaternion);
extern inline void bgc_quaternion_conjugate_fp64(BgcQuaternionFP64* quaternion);
extern inline void bgc_quaternion_conjugate_fp32(const BgcQuaternionFP32* quaternion, BgcQuaternionFP32* conjugate);
extern inline void bgc_quaternion_conjugate_fp64(const BgcQuaternionFP64* quaternion, BgcQuaternionFP64* conjugate);
extern inline int bgc_quaternion_normalize_fp32(BgcQuaternionFP32* quaternion);
extern inline int bgc_quaternion_normalize_fp64(BgcQuaternionFP64* quaternion);
extern inline int bgc_quaternion_invert_fp32(const BgcQuaternionFP32* quaternion, BgcQuaternionFP32* inverted);
extern inline int bgc_quaternion_invert_fp64(const BgcQuaternionFP64* quaternion, BgcQuaternionFP64* inverted);
extern inline void bgc_quaternion_get_conjugate_fp32(const BgcQuaternionFP32* quaternion, BgcQuaternionFP32* conjugate);
extern inline void bgc_quaternion_get_conjugate_fp64(const BgcQuaternionFP64* quaternion, BgcQuaternionFP64* conjugate);
extern inline int bgc_quaternion_get_normalized_fp32(const BgcQuaternionFP32* quaternion, BgcQuaternionFP32* normalized);
extern inline int bgc_quaternion_get_normalized_fp64(const BgcQuaternionFP64* quaternion, BgcQuaternionFP64* normalized);
extern inline int bgc_quaternion_normalize_fp32(const BgcQuaternionFP32* quaternion, BgcQuaternionFP32* normalized);
extern inline int bgc_quaternion_normalize_fp64(const BgcQuaternionFP64* quaternion, BgcQuaternionFP64* normalized);
extern inline void bgc_quaternion_get_product_fp32(const BgcQuaternionFP32* left, const BgcQuaternionFP32* right, BgcQuaternionFP32* product);
extern inline void bgc_quaternion_get_product_fp64(const BgcQuaternionFP64* left, const BgcQuaternionFP64* right, BgcQuaternionFP64* product);
extern inline void bgc_quaternion_get_rotation_matrix_fp32(const BgcQuaternionFP32* quaternion, BgcMatrix3x3FP32* rotation);
extern inline void bgc_quaternion_get_rotation_matrix_fp64(const BgcQuaternionFP64* quaternion, BgcMatrix3x3FP64* rotation);
extern inline void bgc_quaternion_get_reverse_matrix_fp32(const BgcQuaternionFP32* quaternion, BgcMatrix3x3FP32* reverse);
extern inline void bgc_quaternion_get_reverse_matrix_fp64(const BgcQuaternionFP64* quaternion, BgcMatrix3x3FP64* reverse);
extern inline int bgc_quaternion_get_ratio_fp32(const BgcQuaternionFP32* divident, const BgcQuaternionFP32* divisor, BgcQuaternionFP32* quotient);
extern inline int bgc_quaternion_get_ratio_fp64(const BgcQuaternionFP64* divident, const BgcQuaternionFP64* divisor, BgcQuaternionFP64* quotient);
extern inline void bgc_quaternion_add_fp32(const BgcQuaternionFP32* quaternion1, const BgcQuaternionFP32* quaternion2, BgcQuaternionFP32* sum);
extern inline void bgc_quaternion_add_fp64(const BgcQuaternionFP64* quaternion1, const BgcQuaternionFP64* quaternion2, BgcQuaternionFP64* sum);
@ -72,5 +66,11 @@ extern inline void bgc_quaternion_divide_fp64(const BgcQuaternionFP64* dividend,
extern inline void bgc_quaternion_get_linear_interpolation_fp32(const BgcQuaternionFP32* vector1, const BgcQuaternionFP32* vector2, const float phase, BgcQuaternionFP32* interpolation);
extern inline void bgc_quaternion_get_linear_interpolation_fp64(const BgcQuaternionFP64* vector1, const BgcQuaternionFP64* vector2, const double phase, BgcQuaternionFP64* interpolation);
extern inline int bgc_quaternion_get_rotation_matrix_fp32(const BgcQuaternionFP32* quaternion, BgcMatrix3x3FP32* rotation);
extern inline int bgc_quaternion_get_rotation_matrix_fp64(const BgcQuaternionFP64* quaternion, BgcMatrix3x3FP64* rotation);
extern inline int bgc_quaternion_get_reverse_matrix_fp32(const BgcQuaternionFP32* quaternion, BgcMatrix3x3FP32* reverse);
extern inline int bgc_quaternion_get_reverse_matrix_fp64(const BgcQuaternionFP64* quaternion, BgcMatrix3x3FP64* reverse);
extern inline int bgc_quaternion_are_close_fp32(const BgcQuaternionFP32* quaternion1, const BgcQuaternionFP32* quaternion2);
extern inline int bgc_quaternion_are_close_fp32(const BgcQuaternionFP32* quaternion1, const BgcQuaternionFP32* quaternion2);

508
basic-geometry/quaternion.h
View file

Internal server error - Personal Git Server: Beyond coding. We Forge.

500

Internal server error

Forgejo version: 11.0.1+gitea-1.22.0

 @ -119,20 +119,20 @@ inline int bgc_quaternion_is_unit_fp64(const BgcQuaternionFP64* quaternion)
// ==================== Copy ==================== //
inline void bgc_quaternion_copy_fp32(const BgcQuaternionFP32* from, BgcQuaternionFP32* to)
inline void bgc_quaternion_copy_fp32(const BgcQuaternionFP32* source, BgcQuaternionFP32* destination)
{
to->s0 = from->s0;
to->x1 = from->x1;
to->x2 = from->x2;
to->x3 = from->x3;
destination->s0 = source->s0;
destination->x1 = source->x1;
destination->x2 = source->x2;
destination->x3 = source->x3;
}
inline void bgc_quaternion_copy_fp64(const BgcQuaternionFP64* from, BgcQuaternionFP64* to)
inline void bgc_quaternion_copy_fp64(const BgcQuaternionFP64* source, BgcQuaternionFP64* destination)
{
to->s0 = from->s0;
to->x1 = from->x1;
to->x2 = from->x2;
to->x3 = from->x3;
destination->s0 = source->s0;
destination->x1 = source->x1;
destination->x2 = source->x2;
destination->x3 = source->x3;
}
// ==================== Swap ==================== //
@ -175,119 +175,127 @@ inline void bgc_quaternion_swap_fp64(BgcQuaternionFP64* quarternion1, BgcQuatern
// ================== Convert =================== //
inline void bgc_quaternion_convert_fp64_to_fp32(const BgcQuaternionFP64* quaternion, BgcQuaternionFP32* result)
inline void bgc_quaternion_convert_fp64_to_fp32(const BgcQuaternionFP64* source, BgcQuaternionFP32* destination)
{
result->s0 = (float) quaternion->s0;
result->x1 = (float) quaternion->x1;
result->x2 = (float) quaternion->x2;
result->x3 = (float) quaternion->x3;
destination->s0 = (float)source->s0;
destination->x1 = (float)source->x1;
destination->x2 = (float)source->x2;
destination->x3 = (float)source->x3;
}
inline void bgc_quaternion_convert_fp32_to_fp64(const BgcQuaternionFP32* quaternion, BgcQuaternionFP64* result)
inline void bgc_quaternion_convert_fp32_to_fp64(const BgcQuaternionFP32* source, BgcQuaternionFP64* destination)
{
result->s0 = quaternion->s0;
result->x1 = quaternion->x1;
result->x2 = quaternion->x2;
result->x3 = quaternion->x3;
destination->s0 = source->s0;
destination->x1 = source->x1;
destination->x2 = source->x2;
destination->x3 = source->x3;
}
// ================= Conjugate ================== //
inline void bgc_quaternion_conjugate_fp32(BgcQuaternionFP32* quaternion)
inline void bgc_quaternion_conjugate_fp32(const BgcQuaternionFP32* quaternion, BgcQuaternionFP32* conjugate)
{
quaternion->x1 = -quaternion->x1;
quaternion->x2 = -quaternion->x2;
quaternion->x3 = -quaternion->x3;
conjugate->s0 = quaternion->s0;
conjugate->x1 = -quaternion->x1;
conjugate->x2 = -quaternion->x2;
conjugate->x3 = -quaternion->x3;
}
inline void bgc_quaternion_conjugate_fp64(BgcQuaternionFP64* quaternion)
inline void bgc_quaternion_conjugate_fp64(const BgcQuaternionFP64* quaternion, BgcQuaternionFP64* conjugate)
{
quaternion->x1 = -quaternion->x1;
quaternion->x2 = -quaternion->x2;
quaternion->x3 = -quaternion->x3;
conjugate->s0 = quaternion->s0;
conjugate->x1 = -quaternion->x1;
conjugate->x2 = -quaternion->x2;
conjugate->x3 = -quaternion->x3;
}
// =================== Invert =================== //
inline int bgc_quaternion_invert_fp32(const BgcQuaternionFP32* quaternion, BgcQuaternionFP32* inverted)
{
const float square_modulus = bgc_quaternion_get_square_modulus_fp32(quaternion);
if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) {
return 0;
}
const float multiplicand = 1.0f / square_modulus;
inverted->s0 = quaternion->s0 * multiplicand;
inverted->x1 = -quaternion->x1 * multiplicand;
inverted->x2 = -quaternion->x2 * multiplicand;
inverted->x3 = -quaternion->x3 * multiplicand;
return 1;
}
inline int bgc_quaternion_invert_fp64(const BgcQuaternionFP64* quaternion, BgcQuaternionFP64* inverted)
{
const double square_modulus = bgc_quaternion_get_square_modulus_fp64(quaternion);
if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) {
return 0;
}
const double multiplicand = 1.0 / square_modulus;
inverted->s0 = quaternion->s0 * multiplicand;
inverted->x1 = -quaternion->x1 * multiplicand;
inverted->x2 = -quaternion->x2 * multiplicand;
inverted->x3 = -quaternion->x3 * multiplicand;
return 1;
}
// ================= Normalize ================== //
inline int bgc_quaternion_normalize_fp32(BgcQuaternionFP32* quaternion)
inline int bgc_quaternion_normalize_fp32(const BgcQuaternionFP32* quaternion, BgcQuaternionFP32* normalized)
{
const float square_modulus = bgc_quaternion_get_square_modulus_fp32(quaternion);
if (bgc_is_sqare_unit_fp32(square_modulus)) {
bgc_quaternion_copy_fp32(quaternion, normalized);
return 1;
}
if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) {
bgc_quaternion_reset_fp32(quaternion);
return 0;
}
const float multiplier = sqrtf(1.0f / square_modulus);
quaternion->s0 *= multiplier;
quaternion->x1 *= multiplier;
quaternion->x2 *= multiplier;
quaternion->x3 *= multiplier;
normalized->s0 = quaternion->s0 * multiplier;
normalized->x1 = quaternion->x1 * multiplier;
normalized->x2 = quaternion->x2 * multiplier;
normalized->x3 = quaternion->x3 * multiplier;
return 1;
}
inline int bgc_quaternion_normalize_fp64(BgcQuaternionFP64* quaternion)
inline int bgc_quaternion_normalize_fp64(const BgcQuaternionFP64* quaternion, BgcQuaternionFP64* normalized)
{
const double square_modulus = bgc_quaternion_get_square_modulus_fp64(quaternion);
if (bgc_is_sqare_unit_fp64(square_modulus)) {
bgc_quaternion_copy_fp64(quaternion, normalized);
return 1;
}
if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) {
bgc_quaternion_reset_fp64(quaternion);
return 0;
}
const double multiplier = sqrt(1.0 / square_modulus);
quaternion->s0 *= multiplier;
quaternion->x1 *= multiplier;
quaternion->x2 *= multiplier;
quaternion->x3 *= multiplier;
normalized->s0 *= multiplier;
normalized->x1 *= multiplier;
normalized->x2 *= multiplier;
normalized->x3 *= multiplier;
return 1;
}
// =============== Get Conjugate ================ //
inline void bgc_quaternion_get_conjugate_fp32(const BgcQuaternionFP32* quaternion, BgcQuaternionFP32* conjugate)
{
conjugate->s0 = quaternion->s0;
conjugate->x1 = -quaternion->x1;
conjugate->x2 = -quaternion->x2;
conjugate->x3 = -quaternion->x3;
}
inline void bgc_quaternion_get_conjugate_fp64(const BgcQuaternionFP64* quaternion, BgcQuaternionFP64* conjugate)
{
conjugate->s0 = quaternion->s0;
conjugate->x1 = -quaternion->x1;
conjugate->x2 = -quaternion->x2;
conjugate->x3 = -quaternion->x3;
}
// =============== Get Normalized =============== //
inline int bgc_quaternion_get_normalized_fp32(const BgcQuaternionFP32* quaternion, BgcQuaternionFP32* normalized)
{
bgc_quaternion_copy_fp32(quaternion, normalized);
return bgc_quaternion_normalize_fp32(normalized);
}
inline int bgc_quaternion_get_normalized_fp64(const BgcQuaternionFP64* quaternion, BgcQuaternionFP64* normalized)
{
bgc_quaternion_copy_fp64(quaternion, normalized);
return bgc_quaternion_normalize_fp64(normalized);
}
// ================== Product =================== //
// ================ Get Product ================= //
inline void bgc_quaternion_get_product_fp32(const BgcQuaternionFP32* left, const BgcQuaternionFP32* right, BgcQuaternionFP32* product)
{
@ -315,164 +323,52 @@ inline void bgc_quaternion_get_product_fp64(const BgcQuaternionFP64* left, const
product->x3 = x3;
}
// ============ Get Rotation Matrix ============= //
// ================= Get Ratio ================== //
inline void bgc_quaternion_get_rotation_matrix_fp32(const BgcQuaternionFP32* quaternion, BgcMatrix3x3FP32* rotation)
inline int bgc_quaternion_get_ratio_fp32(const BgcQuaternionFP32* divident, const BgcQuaternionFP32* divisor, BgcQuaternionFP32* quotient)
{
const float s0s0 = quaternion->s0 * quaternion->s0;
const float x1x1 = quaternion->x1 * quaternion->x1;
const float x2x2 = quaternion->x2 * quaternion->x2;
const float x3x3 = quaternion->x3 * quaternion->x3;
const float square_modulus = bgc_quaternion_get_square_modulus_fp32(divisor);
const float square_modulus = (s0s0 + x1x1) + (x2x2 + x3x3);
if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus)
{
bgc_matrix3x3_set_to_identity_fp32(rotation);
return;
if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) {
return 0;
}
const float corrector1 = 1.0f / square_modulus;
const float s0 = (divident->s0 * divisor->s0 + divident->x1 * divisor->x1) + (divident->x2 * divisor->x2 + divident->x3 * divisor->x3);
const float x1 = (divident->x1 * divisor->s0 + divident->x3 * divisor->x2) - (divident->s0 * divisor->x1 + divident->x2 * divisor->x3);
const float x2 = (divident->x2 * divisor->s0 + divident->x1 * divisor->x3) - (divident->s0 * divisor->x2 + divident->x3 * divisor->x1);
const float x3 = (divident->x3 * divisor->s0 + divident->x2 * divisor->x1) - (divident->s0 * divisor->x3 + divident->x1 * divisor->x2);
const float s0x1 = quaternion->s0 * quaternion->x1;
const float s0x2 = quaternion->s0 * quaternion->x2;
const float s0x3 = quaternion->s0 * quaternion->x3;
const float x1x2 = quaternion->x1 * quaternion->x2;
const float x1x3 = quaternion->x1 * quaternion->x3;
const float x2x3 = quaternion->x2 * quaternion->x3;
const float multiplicand = 1.0f / square_modulus;
const float corrector2 = 2.0f * corrector1;
quotient->s0 = s0 * multiplicand;
quotient->x1 = x1 * multiplicand;
quotient->x2 = x2 * multiplicand;
quotient->x3 = x3 * multiplicand;
rotation->r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3));
rotation->r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3));
rotation->r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2));
rotation->r1c2 = corrector2 * (x1x2 - s0x3);
rotation->r2c3 = corrector2 * (x2x3 - s0x1);
rotation->r3c1 = corrector2 * (x1x3 - s0x2);
rotation->r2c1 = corrector2 * (x1x2 + s0x3);
rotation->r3c2 = corrector2 * (x2x3 + s0x1);
rotation->r1c3 = corrector2 * (x1x3 + s0x2);
return 1;
}
inline void bgc_quaternion_get_rotation_matrix_fp64(const BgcQuaternionFP64* quaternion, BgcMatrix3x3FP64* rotation)
inline int bgc_quaternion_get_ratio_fp64(const BgcQuaternionFP64* divident, const BgcQuaternionFP64* divisor, BgcQuaternionFP64* quotient)
{
const double s0s0 = quaternion->s0 * quaternion->s0;
const double x1x1 = quaternion->x1 * quaternion->x1;
const double x2x2 = quaternion->x2 * quaternion->x2;
const double x3x3 = quaternion->x3 * quaternion->x3;
const double square_modulus = bgc_quaternion_get_square_modulus_fp64(divisor);
const double square_modulus = (s0s0 + x1x1) + (x2x2 + x3x3);
if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus)
{
bgc_matrix3x3_set_to_identity_fp64(rotation);
return;
if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) {
return 0;
}
const double corrector1 = 1.0f / square_modulus;
const double s0 = (divident->s0 * divisor->s0 + divident->x1 * divisor->x1) + (divident->x2 * divisor->x2 + divident->x3 * divisor->x3);
const double x1 = (divident->x1 * divisor->s0 + divident->x3 * divisor->x2) - (divident->s0 * divisor->x1 + divident->x2 * divisor->x3);
const double x2 = (divident->x2 * divisor->s0 + divident->x1 * divisor->x3) - (divident->s0 * divisor->x2 + divident->x3 * divisor->x1);
const double x3 = (divident->x3 * divisor->s0 + divident->x2 * divisor->x1) - (divident->s0 * divisor->x3 + divident->x1 * divisor->x2);
const double s0x1 = quaternion->s0 * quaternion->x1;
const double s0x2 = quaternion->s0 * quaternion->x2;
const double s0x3 = quaternion->s0 * quaternion->x3;
const double x1x2 = quaternion->x1 * quaternion->x2;
const double x1x3 = quaternion->x1 * quaternion->x3;
const double x2x3 = quaternion->x2 * quaternion->x3;
const double multiplicand = 1.0 / square_modulus;
const double corrector2 = 2.0f * corrector1;
quotient->s0 = s0 * multiplicand;
quotient->x1 = x1 * multiplicand;
quotient->x2 = x2 * multiplicand;
quotient->x3 = x3 * multiplicand;
rotation->r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3));
rotation->r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3));
rotation->r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2));
rotation->r1c2 = corrector2 * (x1x2 - s0x3);
rotation->r2c3 = corrector2 * (x2x3 - s0x1);
rotation->r3c1 = corrector2 * (x1x3 - s0x2);
rotation->r2c1 = corrector2 * (x1x2 + s0x3);
rotation->r3c2 = corrector2 * (x2x3 + s0x1);
rotation->r1c3 = corrector2 * (x1x3 + s0x2);
}
// ============= Get Reverse Matrix ============= //
inline void bgc_quaternion_get_reverse_matrix_fp32(const BgcQuaternionFP32* quaternion, BgcMatrix3x3FP32* reverse)
{
const float s0s0 = quaternion->s0 * quaternion->s0;
const float x1x1 = quaternion->x1 * quaternion->x1;
const float x2x2 = quaternion->x2 * quaternion->x2;
const float x3x3 = quaternion->x3 * quaternion->x3;
const float square_modulus = (s0s0 + x1x1) + (x2x2 + x3x3);
if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus)
{
bgc_matrix3x3_set_to_identity_fp32(reverse);
return;
}
const float corrector1 = 1.0f / square_modulus;
const float s0x1 = quaternion->s0 * quaternion->x1;
const float s0x2 = quaternion->s0 * quaternion->x2;
const float s0x3 = quaternion->s0 * quaternion->x3;
const float x1x2 = quaternion->x1 * quaternion->x2;
const float x1x3 = quaternion->x1 * quaternion->x3;
const float x2x3 = quaternion->x2 * quaternion->x3;
const float corrector2 = 2.0f * corrector1;
reverse->r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3));
reverse->r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3));
reverse->r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2));
reverse->r1c2 = corrector2 * (x1x2 + s0x3);
reverse->r2c3 = corrector2 * (x2x3 + s0x1);
reverse->r3c1 = corrector2 * (x1x3 + s0x2);
reverse->r2c1 = corrector2 * (x1x2 - s0x3);
reverse->r3c2 = corrector2 * (x2x3 - s0x1);
reverse->r1c3 = corrector2 * (x1x3 - s0x2);
}
inline void bgc_quaternion_get_reverse_matrix_fp64(const BgcQuaternionFP64* quaternion, BgcMatrix3x3FP64* reverse)
{
const double s0s0 = quaternion->s0 * quaternion->s0;
const double x1x1 = quaternion->x1 * quaternion->x1;
const double x2x2 = quaternion->x2 * quaternion->x2;
const double x3x3 = quaternion->x3 * quaternion->x3;
const double square_modulus = (s0s0 + x1x1) + (x2x2 + x3x3);
if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus)
{
bgc_matrix3x3_set_to_identity_fp64(reverse);
return;
}
const double corrector1 = 1.0f / square_modulus;
const double s0x1 = quaternion->s0 * quaternion->x1;
const double s0x2 = quaternion->s0 * quaternion->x2;
const double s0x3 = quaternion->s0 * quaternion->x3;
const double x1x2 = quaternion->x1 * quaternion->x2;
const double x1x3 = quaternion->x1 * quaternion->x3;
const double x2x3 = quaternion->x2 * quaternion->x3;
const double corrector2 = 2.0f * corrector1;
reverse->r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3));
reverse->r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3));
reverse->r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2));
reverse->r1c2 = corrector2 * (x1x2 + s0x3);
reverse->r2c3 = corrector2 * (x2x3 + s0x1);
reverse->r3c1 = corrector2 * (x1x3 + s0x2);
reverse->r2c1 = corrector2 * (x1x2 - s0x3);