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

63
basic-geometry/rigid-pose3.h
View file

@ -4,6 +4,7 @@
#include <math.h>
#include "types.h"
#include "affine3.h"
#include "quaternion.h"
#include "dual-quaternion.h"
@ -235,4 +236,66 @@ inline void bgc_fp64_rigid_pose3_exclude(BGC_FP64_RigidPose3* difference, const
_bgc_fp64_rigid_pose3_normalize(difference);
}
// ============= Get Outward Matrix ============= //
inline void bgc_fp32_rigid_pose3_get_outward_matrix(BGC_FP32_Matrix3x3* matrix, const BGC_FP32_RigidPose3* pose)
{
_bgc_fp32_versor_get_rotation_matrix(matrix, &pose->_versor.real_part);
}
inline void bgc_fp64_rigid_pose3_get_outward_matrix(BGC_FP64_Matrix3x3* matrix, const BGC_FP64_RigidPose3* pose)
{
_bgc_fp64_versor_get_rotation_matrix(matrix, &pose->_versor.real_part);
}
// ============= Get Inward Matrix ============== //
inline void bgc_fp32_rigid_pose3_get_inward_matrix(BGC_FP32_Matrix3x3* matrix, const BGC_FP32_RigidPose3* pose)
{
_bgc_fp32_versor_get_reverse_matrix(matrix, &pose->_versor.real_part);
}
inline void bgc_fp64_rigid_pose3_get_inward_matrix(BGC_FP64_Matrix3x3* matrix, const BGC_FP64_RigidPose3* pose)
{
_bgc_fp64_versor_get_reverse_matrix(matrix, &pose->_versor.real_part);
}
// ============ Get Outward Affine3 ============= //
inline void bgc_fp32_rigid_pose3_get_outward_affine3(BGC_FP32_Affine3* affine_map, const BGC_FP32_RigidPose3* pose)
{
_bgc_fp32_versor_get_rotation_matrix(&affine_map->distortion, &pose->_versor.real_part);
const BGC_FP32_Quaternion * real = &pose->_versor.real_part;
const BGC_FP32_Quaternion * dual = &pose->_versor.dual_part;
affine_map->shift.x1 = (dual->x1 * real->s0 + dual->x3 * real->x2) - (dual->s0 * real->x1 + dual->x2 * real->x3);
affine_map->shift.x2 = (dual->x2 * real->s0 + dual->x1 * real->x3) - (dual->s0 * real->x2 + dual->x3 * real->x1);
affine_map->shift.x3 = (dual->x3 * real->s0 + dual->x2 * real->x1) - (dual->s0 * real->x3 + dual->x1 * real->x2);
}
inline void bgc_fp64_rigid_pose3_get_outward_affine3(BGC_FP64_Affine3* affine_map, const BGC_FP64_RigidPose3* pose)
{
_bgc_fp64_versor_get_rotation_matrix(&affine_map->distortion, &pose->_versor.real_part);
const BGC_FP64_Quaternion * real = &pose->_versor.real_part;
const BGC_FP64_Quaternion * dual = &pose->_versor.dual_part;
affine_map->shift.x1 = (dual->x1 * real->s0 + dual->x3 * real->x2) - (dual->s0 * real->x1 + dual->x2 * real->x3);
affine_map->shift.x2 = (dual->x2 * real->s0 + dual->x1 * real->x3) - (dual->s0 * real->x2 + dual->x3 * real->x1);
affine_map->shift.x3 = (dual->x3 * real->s0 + dual->x2 * real->x1) - (dual->s0 * real->x3 + dual->x1 * real->x2);
}
// ============= Get Inward Affine3 ============= //
inline void bgc_fp32_rigid_pose3_get_inward_affine3(BGC_FP32_Affine3* affine_map, const BGC_FP32_RigidPose3* pose)
{
_bgc_fp32_versor_get_reverse_matrix(&affine_map->distortion, &pose->_versor.real_part);
const BGC_FP32_Quaternion * real = &pose->_versor.real_part;
const BGC_FP32_Quaternion * dual = &pose->_versor.dual_part;
//TODO: set the shift in the affine map
}
#endif