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

Реорганизация функций для получения матрицы повотора и матрицы обратного поворота для версоров, добавлены функции получения матриц поворота и аффинных преобразований для Rigid Pose3

Browse source
This commit is contained in:
Andrey Pokidov 2026-03-20 20:57:09 +07:00
parent b2af8408ce
commit 4ead7ca106
6 changed files with 220 additions and 116 deletions
Download patch file Download diff file Expand all files Collapse all files

116
basic-geometry/turn3.h
View file

@ -381,133 +381,37 @@ void bgc_fp64_turn3_spherically_interpolate(BGC_FP64_Turn3* interpolation, const
inline void bgc_fp32_turn3_get_rotation_matrix(BGC_FP32_Matrix3x3* matrix, const BGC_FP32_Turn3* turn)
{
const float s0s0 = turn->_versor.s0 * turn->_versor.s0;
const float x1x1 = turn->_versor.x1 * turn->_versor.x1;
const float x2x2 = turn->_versor.x2 * turn->_versor.x2;
const float x3x3 = turn->_versor.x3 * turn->_versor.x3;
const float s0x1 = turn->_versor.s0 * turn->_versor.x1;
const float s0x2 = turn->_versor.s0 * turn->_versor.x2;
const float s0x3 = turn->_versor.s0 * turn->_versor.x3;
const float x1x2 = turn->_versor.x1 * turn->_versor.x2;
const float x1x3 = turn->_versor.x1 * turn->_versor.x3;
const float x2x3 = turn->_versor.x2 * turn->_versor.x3;
matrix->r1c1 = ((s0s0 + x1x1) - (x2x2 + x3x3));
matrix->r2c2 = ((s0s0 + x2x2) - (x1x1 + x3x3));
matrix->r3c3 = ((s0s0 + x3x3) - (x1x1 + x2x2));
matrix->r1c2 = 2.0f * (x1x2 - s0x3);
matrix->r2c3 = 2.0f * (x2x3 - s0x1);
matrix->r3c1 = 2.0f * (x1x3 - s0x2);
matrix->r2c1 = 2.0f * (x1x2 + s0x3);
matrix->r3c2 = 2.0f * (x2x3 + s0x1);
matrix->r1c3 = 2.0f * (x1x3 + s0x2);
_bgc_fp32_versor_get_rotation_matrix(matrix, &turn->_versor);
}
inline void bgc_fp64_turn3_get_rotation_matrix(BGC_FP64_Matrix3x3* matrix, const BGC_FP64_Turn3* turn)
{
const double s0s0 = turn->_versor.s0 * turn->_versor.s0;
const double x1x1 = turn->_versor.x1 * turn->_versor.x1;
const double x2x2 = turn->_versor.x2 * turn->_versor.x2;
const double x3x3 = turn->_versor.x3 * turn->_versor.x3;
const double s0x1 = turn->_versor.s0 * turn->_versor.x1;
const double s0x2 = turn->_versor.s0 * turn->_versor.x2;
const double s0x3 = turn->_versor.s0 * turn->_versor.x3;
const double x1x2 = turn->_versor.x1 * turn->_versor.x2;
const double x1x3 = turn->_versor.x1 * turn->_versor.x3;
const double x2x3 = turn->_versor.x2 * turn->_versor.x3;
matrix->r1c1 = ((s0s0 + x1x1) - (x2x2 + x3x3));
matrix->r2c2 = ((s0s0 + x2x2) - (x1x1 + x3x3));
matrix->r3c3 = ((s0s0 + x3x3) - (x1x1 + x2x2));
matrix->r1c2 = 2.0 * (x1x2 - s0x3);
matrix->r2c3 = 2.0 * (x2x3 - s0x1);
matrix->r3c1 = 2.0 * (x1x3 - s0x2);
matrix->r2c1 = 2.0 * (x1x2 + s0x3);
matrix->r3c2 = 2.0 * (x2x3 + s0x1);
matrix->r1c3 = 2.0 * (x1x3 + s0x2);
_bgc_fp64_versor_get_rotation_matrix(matrix, &turn->_versor);
}
// ============= Get Reverse Matrix ============= //
inline void bgc_fp32_turn3_get_reverse_matrix(BGC_FP32_Matrix3x3* matrix, const BGC_FP32_Turn3* turn)
{
const float s0s0 = turn->_versor.s0 * turn->_versor.s0;
const float x1x1 = turn->_versor.x1 * turn->_versor.x1;
const float x2x2 = turn->_versor.x2 * turn->_versor.x2;
const float x3x3 = turn->_versor.x3 * turn->_versor.x3;
const float s0x1 = turn->_versor.s0 * turn->_versor.x1;
const float s0x2 = turn->_versor.s0 * turn->_versor.x2;
const float s0x3 = turn->_versor.s0 * turn->_versor.x3;
const float x1x2 = turn->_versor.x1 * turn->_versor.x2;
const float x1x3 = turn->_versor.x1 * turn->_versor.x3;
const float x2x3 = turn->_versor.x2 * turn->_versor.x3;
matrix->r1c1 = ((s0s0 + x1x1) - (x2x2 + x3x3));
matrix->r2c2 = ((s0s0 + x2x2) - (x1x1 + x3x3));
matrix->r3c3 = ((s0s0 + x3x3) - (x1x1 + x2x2));
matrix->r1c2 = 2.0f * (x1x2 + s0x3);
matrix->r2c3 = 2.0f * (x2x3 + s0x1);
matrix->r3c1 = 2.0f * (x1x3 + s0x2);
matrix->r2c1 = 2.0f * (x1x2 - s0x3);
matrix->r3c2 = 2.0f * (x2x3 - s0x1);
matrix->r1c3 = 2.0f * (x1x3 - s0x2);
_bgc_fp32_versor_get_reverse_matrix(matrix, &turn->_versor);
}
inline void bgc_fp64_turn3_get_reverse_matrix(BGC_FP64_Matrix3x3* matrix, const BGC_FP64_Turn3* turn)
{
const double s0s0 = turn->_versor.s0 * turn->_versor.s0;
const double x1x1 = turn->_versor.x1 * turn->_versor.x1;
const double x2x2 = turn->_versor.x2 * turn->_versor.x2;
const double x3x3 = turn->_versor.x3 * turn->_versor.x3;
const double s0x1 = turn->_versor.s0 * turn->_versor.x1;
const double s0x2 = turn->_versor.s0 * turn->_versor.x2;
const double s0x3 = turn->_versor.s0 * turn->_versor.x3;
const double x1x2 = turn->_versor.x1 * turn->_versor.x2;
const double x1x3 = turn->_versor.x1 * turn->_versor.x3;
const double x2x3 = turn->_versor.x2 * turn->_versor.x3;
matrix->r1c1 = ((s0s0 + x1x1) - (x2x2 + x3x3));
matrix->r2c2 = ((s0s0 + x2x2) - (x1x1 + x3x3));
matrix->r3c3 = ((s0s0 + x3x3) - (x1x1 + x2x2));
matrix->r1c2 = 2.0 * (x1x2 + s0x3);
matrix->r2c3 = 2.0 * (x2x3 + s0x1);
matrix->r3c1 = 2.0 * (x1x3 + s0x2);
matrix->r2c1 = 2.0 * (x1x2 - s0x3);
matrix->r3c2 = 2.0 * (x2x3 - s0x1);
matrix->r1c3 = 2.0 * (x1x3 - s0x2);
_bgc_fp64_versor_get_reverse_matrix(matrix, &turn->_versor);
}
// ============= Get Both Matrixes ============== //
inline void bgc_fp32_turn3_get_both_matrices(BGC_FP32_Matrix3x3* rotation, BGC_FP32_Matrix3x3* reverse, const BGC_FP32_Turn3* turn)
{
bgc_fp32_turn3_get_reverse_matrix(reverse, turn);
_bgc_fp32_versor_get_reverse_matrix(reverse, &turn->_versor);
bgc_fp32_matrix3x3_get_transposed(rotation, reverse);
}
inline void bgc_fp64_turn3_get_both_matrices(BGC_FP64_Matrix3x3* rotation, BGC_FP64_Matrix3x3* reverse, const BGC_FP64_Turn3* turn)
{
bgc_fp64_turn3_get_reverse_matrix(reverse, turn);
_bgc_fp64_versor_get_reverse_matrix(reverse, &turn->_versor);
bgc_fp64_matrix3x3_get_transposed(rotation, reverse);
}
@ -515,24 +419,24 @@ inline void bgc_fp64_turn3_get_both_matrices(BGC_FP64_Matrix3x3* rotation, BGC_F
inline void bgc_fp32_turn3_vector(BGC_FP32_Vector3* turned_vector, const BGC_FP32_Turn3* turn, const BGC_FP32_Vector3* original_vector)
{
_bgc_fp32_quaternion_turn_vector_roughly(turned_vector, &turn->_versor, original_vector);
_bgc_fp32_versor_turn_vector(turned_vector, &turn->_versor, original_vector);
}
inline void bgc_fp64_turn3_vector(BGC_FP64_Vector3* turned_vector, const BGC_FP64_Turn3* turn, const BGC_FP64_Vector3* original_vector)
{
_bgc_fp64_quaternion_turn_vector_roughly(turned_vector, &turn->_versor, original_vector);
_bgc_fp64_versor_turn_vector(turned_vector, &turn->_versor, original_vector);
}
// ============== Turn Vector Back ============== //
inline void bgc_fp32_turn3_vector_back(BGC_FP32_Vector3* turned_vector, const BGC_FP32_Turn3* turn, const BGC_FP32_Vector3* original_vector)
{
_bgc_fp32_quaternion_turn_vector_back_roughly(turned_vector, &turn->_versor, original_vector);
_bgc_fp32_versor_turn_vector_back(turned_vector, &turn->_versor, original_vector);
}
inline void bgc_fp64_turn3_vector_back(BGC_FP64_Vector3* turned_vector, const BGC_FP64_Turn3* turn, const BGC_FP64_Vector3* original_vector)
{
_bgc_fp64_quaternion_turn_vector_back_roughly(turned_vector, &turn->_versor, original_vector);
_bgc_fp64_versor_turn_vector_back(turned_vector, &turn->_versor, original_vector);
}
// ================== Are Close ================= //