Реорганизация функций для получения матрицы повотора и матрицы обратного поворота для версоров, добавлены функции получения матриц поворота и аффинных преобразований для Rigid Pose3
This commit is contained in:
parent
b2af8408ce
commit
4ead7ca106
6 changed files with 220 additions and 116 deletions
|
|
@ -753,7 +753,7 @@ int bgc_fp64_quaternion_get_power(BGC_FP64_Quaternion* power, const BGC_FP64_Qua
|
|||
// ============== Raw Turn Vector3 ============== //
|
||||
|
||||
// An internal function
|
||||
inline void _bgc_fp32_quaternion_turn_vector_roughly(BGC_FP32_Vector3* turned_vector, const BGC_FP32_Quaternion* quaternion, const BGC_FP32_Vector3* original_vector)
|
||||
inline void _bgc_fp32_versor_turn_vector(BGC_FP32_Vector3* turned_vector, const BGC_FP32_Quaternion* quaternion, const BGC_FP32_Vector3* original_vector)
|
||||
{
|
||||
const float tx1 = 2.0f * (quaternion->x2 * original_vector->x3 - quaternion->x3 * original_vector->x2);
|
||||
const float tx2 = 2.0f * (quaternion->x3 * original_vector->x1 - quaternion->x1 * original_vector->x3);
|
||||
|
|
@ -769,7 +769,7 @@ inline void _bgc_fp32_quaternion_turn_vector_roughly(BGC_FP32_Vector3* turned_ve
|
|||
}
|
||||
|
||||
// An internal function
|
||||
inline void _bgc_fp64_quaternion_turn_vector_roughly(BGC_FP64_Vector3* turned_vector, const BGC_FP64_Quaternion* quaternion, const BGC_FP64_Vector3* original_vector)
|
||||
inline void _bgc_fp64_versor_turn_vector(BGC_FP64_Vector3* turned_vector, const BGC_FP64_Quaternion* quaternion, const BGC_FP64_Vector3* original_vector)
|
||||
{
|
||||
const double tx1 = 2.0f * (quaternion->x2 * original_vector->x3 - quaternion->x3 * original_vector->x2);
|
||||
const double tx2 = 2.0f * (quaternion->x3 * original_vector->x1 - quaternion->x1 * original_vector->x3);
|
||||
|
|
@ -787,7 +787,7 @@ inline void _bgc_fp64_quaternion_turn_vector_roughly(BGC_FP64_Vector3* turned_ve
|
|||
// ========= Raw Turn Vector3 Backwards ========= //
|
||||
|
||||
// An internal function
|
||||
inline void _bgc_fp32_quaternion_turn_vector_back_roughly(BGC_FP32_Vector3* turned_vector, const BGC_FP32_Quaternion* quaternion, const BGC_FP32_Vector3* original_vector)
|
||||
inline void _bgc_fp32_versor_turn_vector_back(BGC_FP32_Vector3* turned_vector, const BGC_FP32_Quaternion* quaternion, const BGC_FP32_Vector3* original_vector)
|
||||
{
|
||||
const float tx1 = 2.0f * (quaternion->x2 * original_vector->x3 - quaternion->x3 * original_vector->x2);
|
||||
const float tx2 = 2.0f * (quaternion->x3 * original_vector->x1 - quaternion->x1 * original_vector->x3);
|
||||
|
|
@ -803,7 +803,7 @@ inline void _bgc_fp32_quaternion_turn_vector_back_roughly(BGC_FP32_Vector3* turn
|
|||
}
|
||||
|
||||
// An internal function
|
||||
inline void _bgc_fp64_quaternion_turn_vector_back_roughly(BGC_FP64_Vector3* turned_vector, const BGC_FP64_Quaternion* quaternion, const BGC_FP64_Vector3* original_vector)
|
||||
inline void _bgc_fp64_versor_turn_vector_back(BGC_FP64_Vector3* turned_vector, const BGC_FP64_Quaternion* quaternion, const BGC_FP64_Vector3* original_vector)
|
||||
{
|
||||
const double tx1 = 2.0f * (quaternion->x2 * original_vector->x3 - quaternion->x3 * original_vector->x2);
|
||||
const double tx2 = 2.0f * (quaternion->x3 * original_vector->x1 - quaternion->x1 * original_vector->x3);
|
||||
|
|
@ -926,6 +926,126 @@ inline int bgc_fp64_quaternion_turn_vector_back(BGC_FP64_Vector3* turned_vector,
|
|||
return BGC_SUCCESS;
|
||||
}
|
||||
|
||||
// ========= Get Versor Rotation Matrix ========= //
|
||||
|
||||
inline void _bgc_fp32_versor_get_rotation_matrix(BGC_FP32_Matrix3x3* matrix, const BGC_FP32_Quaternion* versor)
|
||||
{
|
||||
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 = versor->s0 * versor->x1;
|
||||
const float s0x2 = versor->s0 * versor->x2;
|
||||
const float s0x3 = versor->s0 * versor->x3;
|
||||
|
||||
const float x1x2 = versor->x1 * versor->x2;
|
||||
const float x1x3 = versor->x1 * versor->x3;
|
||||
|
||||
const float x2x3 = versor->x2 * 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);
|
||||
}
|
||||
|
||||
inline void _bgc_fp64_versor_get_rotation_matrix(BGC_FP64_Matrix3x3* matrix, const BGC_FP64_Quaternion* versor)
|
||||
{
|
||||
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 = versor->s0 * versor->x1;
|
||||
const double s0x2 = versor->s0 * versor->x2;
|
||||
const double s0x3 = versor->s0 * versor->x3;
|
||||
|
||||
const double x1x2 = versor->x1 * versor->x2;
|
||||
const double x1x3 = versor->x1 * versor->x3;
|
||||
|
||||
const double x2x3 = versor->x2 * 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);
|
||||
}
|
||||
|
||||
// ========= Get Versor Reverse Matrix ========== //
|
||||
|
||||
inline void _bgc_fp32_versor_get_reverse_matrix(BGC_FP32_Matrix3x3* matrix, const BGC_FP32_Quaternion* versor)
|
||||
{
|
||||
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 = versor->s0 * versor->x1;
|
||||
const float s0x2 = versor->s0 * versor->x2;
|
||||
const float s0x3 = versor->s0 * versor->x3;
|
||||
|
||||
const float x1x2 = versor->x1 * versor->x2;
|
||||
const float x1x3 = versor->x1 * versor->x3;
|
||||
|
||||
const float x2x3 = versor->x2 * 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);
|
||||
}
|
||||
|
||||
inline void _bgc_fp64_versor_get_reverse_matrix(BGC_FP64_Matrix3x3* matrix, const BGC_FP64_Quaternion* versor)
|
||||
{
|
||||
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 = versor->s0 * versor->x1;
|
||||
const double s0x2 = versor->s0 * versor->x2;
|
||||
const double s0x3 = versor->s0 * versor->x3;
|
||||
|
||||
const double x1x2 = versor->x1 * versor->x2;
|
||||
const double x1x3 = versor->x1 * versor->x3;
|
||||
|
||||
const double x2x3 = versor->x2 * 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);
|
||||
}
|
||||
|
||||
// ============ Get Rotation Matrix ============= //
|
||||
|
||||
inline int bgc_fp32_quaternion_get_rotation_matrix(BGC_FP32_Matrix3x3* rotation, const BGC_FP32_Quaternion* quaternion)
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue