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

Добавление операций проверки состояний кватернионов и дуальных кватернионов, а также операций трансформации вектора с помощью rigid pose

Browse source
This commit is contained in:
Andrey Pokidov 2026-03-26 01:49:18 +07:00
parent dc759e61f7
commit 405af2f3a7
8 changed files with 152 additions and 32 deletions
Download patch file Download diff file Expand all files Collapse all files

2
basic-geometry/position3.h
View file

@ -209,7 +209,6 @@ inline void bgc_fp32_position3_transform_point_back(BGC_FP32_Vector3* const tran
BGC_FP32_Vector3 relative_point;
bgc_fp32_vector3_subtract(&relative_point, initial_point, &position->shift);
bgc_fp32_turn3_vector_back(transformed_point, &position->turn, &relative_point);
}
inline void bgc_fp64_position3_transform_point_back(BGC_FP64_Vector3* const transformed_point, const BGC_FP64_Position3* const position, const BGC_FP64_Vector3* const initial_point)
@ -217,7 +216,6 @@ inline void bgc_fp64_position3_transform_point_back(BGC_FP64_Vector3* const tran
BGC_FP64_Vector3 relative_point;
bgc_fp64_vector3_subtract(&relative_point, initial_point, &position->shift);
bgc_fp64_turn3_vector_back(transformed_point, &position->turn, &relative_point);
}
// ============== Transform Vector =============== //

6
basic-geometry/quaternion.c
View file

@ -22,6 +22,12 @@ extern inline int bgc_fp64_quaternion_is_zero(const BGC_FP64_Quaternion* const q
extern inline int bgc_fp32_quaternion_is_unit(const BGC_FP32_Quaternion* const quaternion);
extern inline int bgc_fp64_quaternion_is_unit(const BGC_FP64_Quaternion* const quaternion);
extern inline int bgc_fp32_quaternion_is_pure(const BGC_FP32_Quaternion* const quaternion);
extern inline int bgc_fp64_quaternion_is_pure(const BGC_FP64_Quaternion* const quaternion);
extern inline int bgc_fp32_quaternion_is_real(const BGC_FP32_Quaternion* const quaternion);
extern inline int bgc_fp64_quaternion_is_real(const BGC_FP64_Quaternion* const quaternion);
extern inline void bgc_fp32_quaternion_copy(BGC_FP32_Quaternion* const destination, const BGC_FP32_Quaternion* const source);
extern inline void bgc_fp64_quaternion_copy(BGC_FP64_Quaternion* const destination, const BGC_FP64_Quaternion* const source);

24
basic-geometry/quaternion.h
View file

@ -110,6 +110,30 @@ inline int bgc_fp64_quaternion_is_unit(const BGC_FP64_Quaternion* const quaterni
return bgc_fp64_is_square_unit(bgc_fp64_quaternion_get_square_magnitude(quaternion));
}
// ================== Is Pure =================== //
inline int bgc_fp32_quaternion_is_pure(const BGC_FP32_Quaternion* const quaternion)
{
return bgc_fp32_is_zero(quaternion->s0);
}
inline int bgc_fp64_quaternion_is_pure(const BGC_FP64_Quaternion* const quaternion)
{
return bgc_fp64_is_zero(quaternion->s0);
}
// ================== Is Real =================== //
inline int bgc_fp32_quaternion_is_real(const BGC_FP32_Quaternion* const quaternion)
{
return quaternion->x1 * quaternion->x1 + quaternion->x2 * quaternion->x2 + quaternion->x3 * quaternion->x3 <= BGC_FP32_SQUARE_EPSILON;
}
inline int bgc_fp64_quaternion_is_real(const BGC_FP64_Quaternion* const quaternion)
{
return quaternion->x1 * quaternion->x1 + quaternion->x2 * quaternion->x2 + quaternion->x3 * quaternion->x3 <= BGC_FP64_SQUARE_EPSILON;
}
// ==================== Copy ==================== //
inline void bgc_fp32_quaternion_copy(BGC_FP32_Quaternion* const destination, const BGC_FP32_Quaternion* const source)

9
basic-geometry/rigid-pose3.c
View file

@ -24,6 +24,9 @@ extern inline void bgc_fp64_rigid_pose3_copy(BGC_FP64_RigidPose3* const destinat
extern inline void bgc_fp32_rigid_pose3_swap(BGC_FP32_RigidPose3* const pose1, BGC_FP32_RigidPose3* const pose2);
extern inline void bgc_fp64_rigid_pose3_swap(BGC_FP64_RigidPose3* const pose1, BGC_FP64_RigidPose3* const pose2);
extern inline int bgc_fp32_rigid_pose3_is_idle(const BGC_FP32_RigidPose3* const pose);
extern inline int bgc_fp64_rigid_pose3_is_idle(const BGC_FP64_RigidPose3* const pose);
extern inline void bgc_fp32_rigid_pose3_convert_to_fp64(BGC_FP64_RigidPose3* const destination, const BGC_FP32_RigidPose3* const source);
extern inline void bgc_fp64_rigid_pose3_convert_to_fp32(BGC_FP32_RigidPose3* const destination, const BGC_FP64_RigidPose3* const source);
@ -80,3 +83,9 @@ extern inline void bgc_fp64_rigid_pose3_get_reverse_position(BGC_FP64_Position3*
extern inline void bgc_fp32_rigid_pose3_set_reverse_position(BGC_FP32_RigidPose3* const pose, const BGC_FP32_Position3* const position);
extern inline void bgc_fp64_rigid_pose3_set_reverse_position(BGC_FP64_RigidPose3* const pose, const BGC_FP64_Position3* const position);
extern inline void bgc_fp32_rigid_pose3_transform_vector(BGC_FP32_Vector3* const turned_vector, const BGC_FP32_RigidPose3* const pose, const BGC_FP32_Vector3* const original_vector);
extern inline void bgc_fp64_rigid_pose3_transform_vector(BGC_FP64_Vector3* const turned_vector, const BGC_FP64_RigidPose3* const pose, const BGC_FP64_Vector3* const original_vector);
extern inline void bgc_fp32_rigid_pose3_transform_vector_back(BGC_FP32_Vector3* const turned_vector, const BGC_FP32_RigidPose3* const pose, const BGC_FP32_Vector3* const original_vector);
extern inline void bgc_fp64_rigid_pose3_transform_vector_back(BGC_FP64_Vector3* const turned_vector, const BGC_FP64_RigidPose3* const pose, const BGC_FP64_Vector3* const original_vector);

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

@ -152,6 +152,18 @@ inline void bgc_fp64_rigid_pose3_swap(BGC_FP64_RigidPose3* const pose1, BGC_FP64
bgc_fp64_dual_quaternion_swap(&pose1->_versor, &pose2->_versor);
}
// ================== Is Idle =================== //
inline int bgc_fp32_rigid_pose3_is_idle(const BGC_FP32_RigidPose3* const pose)
{
return bgc_fp32_quaternion_is_real(&pose->_versor.real_part) && bgc_fp32_quaternion_is_zero(&pose->_versor.dual_part);
}
inline int bgc_fp64_rigid_pose3_is_idle(const BGC_FP64_RigidPose3* const pose)
{
return bgc_fp64_quaternion_is_real(&pose->_versor.real_part) && bgc_fp64_quaternion_is_zero(&pose->_versor.dual_part);
}
// ================== Convert =================== //
inline void bgc_fp32_rigid_pose3_convert_to_fp64(BGC_FP64_RigidPose3* const destination, const BGC_FP32_RigidPose3* const source)
@ -488,4 +500,28 @@ inline void bgc_fp64_rigid_pose3_set_reverse_position(BGC_FP64_RigidPose3* const
pose->_versor.dual_part.x3 = -0.5f * (versor->s0 * shift->x3 + versor->x2 * shift->x1 - versor->x1 * shift->x2);
}
// ============== Transform Vector ============== //
inline void bgc_fp32_rigid_pose3_transform_vector(BGC_FP32_Vector3* const turned_vector, const BGC_FP32_RigidPose3* const pose, const BGC_FP32_Vector3* const original_vector)
{
_bgc_fp32_versor_turn_vector(turned_vector, &pose->_versor.real_part, original_vector);
}
inline void bgc_fp64_rigid_pose3_transform_vector(BGC_FP64_Vector3* const turned_vector, const BGC_FP64_RigidPose3* const pose, const BGC_FP64_Vector3* const original_vector)
{
_bgc_fp64_versor_turn_vector(turned_vector, &pose->_versor.real_part, original_vector);
}
// =========== Transform Vector Back ============ //
inline void bgc_fp32_rigid_pose3_transform_vector_back(BGC_FP32_Vector3* const turned_vector, const BGC_FP32_RigidPose3* const pose, const BGC_FP32_Vector3* const original_vector)
{
_bgc_fp32_versor_turn_vector_back(turned_vector, &pose->_versor.real_part, original_vector);
}
inline void bgc_fp64_rigid_pose3_transform_vector_back(BGC_FP64_Vector3* const turned_vector, const BGC_FP64_RigidPose3* const pose, const BGC_FP64_Vector3* const original_vector)
{
_bgc_fp64_versor_turn_vector_back(turned_vector, &pose->_versor.real_part, original_vector);
}
#endif

3
basic-geometry/turn2.c
View file

@ -43,6 +43,9 @@ extern inline void bgc_fp64_turn2_get_power(BGC_FP64_Turn2* const power, const B
extern inline void bgc_fp32_turn2_combine(BGC_FP32_Turn2* const combination, const BGC_FP32_Turn2* const turn1, const BGC_FP32_Turn2* const turn2);
extern inline void bgc_fp64_turn2_combine(BGC_FP64_Turn2* const combination, const BGC_FP64_Turn2* const turn1, const BGC_FP64_Turn2* const turn2);
extern inline void bgc_fp32_turn2_combine3(BGC_FP32_Turn2* const combination, const BGC_FP32_Turn2* const turn1, const BGC_FP32_Turn2* const turn2, const BGC_FP32_Turn2* const turn3);
extern inline void bgc_fp64_turn2_combine3(BGC_FP64_Turn2* const combination, const BGC_FP64_Turn2* const turn1, const BGC_FP64_Turn2* const turn2, const BGC_FP64_Turn2* const turn3);
extern inline void bgc_fp32_turn2_exclude(BGC_FP32_Turn2* const difference, const BGC_FP32_Turn2* const base, const BGC_FP32_Turn2* const excludant);
extern inline void bgc_fp64_turn2_exclude(BGC_FP64_Turn2* const difference, const BGC_FP64_Turn2* const base, const BGC_FP64_Turn2* const excludant);

96
basic-geometry/turn2.h
View file

@ -171,14 +171,20 @@ inline void bgc_fp64_turn2_swap(BGC_FP64_Turn2* const turn1, BGC_FP64_Turn2* con
// ================== Convert =================== //
inline void bgc_fp64_turn2_convert_to_fp32(BGC_FP32_Turn2* const destination, const BGC_FP64_Turn2* const source)
{
bgc_fp32_turn2_set_values(destination, (float)source->_cos, (float)source->_sin);
}
inline void bgc_fp32_turn2_convert_to_fp64(BGC_FP64_Turn2* const destination, const BGC_FP32_Turn2* const source)
{
bgc_fp64_turn2_set_values(destination, (double)source->_cos, (double)source->_sin);
destination->_cos = source->_cos;
destination->_sin = source->_sin;
_bgc_fp64_turn2_normalize(destination);
}
inline void bgc_fp64_turn2_convert_to_fp32(BGC_FP32_Turn2* const destination, const BGC_FP64_Turn2* const source)
{
destination->_cos = (float)source->_cos;
destination->_sin = (float)source->_sin;
_bgc_fp32_turn2_normalize(destination);
}
// =================== Revert =================== //
@ -227,40 +233,78 @@ inline void bgc_fp64_turn2_get_power(BGC_FP64_Turn2* const power, const BGC_FP64
inline void bgc_fp32_turn2_combine(BGC_FP32_Turn2* const combination, const BGC_FP32_Turn2* const turn1, const BGC_FP32_Turn2* const turn2)
{
bgc_fp32_turn2_set_values(
combination,
turn1->_cos * turn2->_cos - turn1->_sin * turn2->_sin,
turn1->_cos * turn2->_sin + turn1->_sin * turn2->_cos
);
const float _cos = turn1->_cos * turn2->_cos - turn1->_sin * turn2->_sin;
const float _sin = turn1->_cos * turn2->_sin + turn1->_sin * turn2->_cos;
combination->_cos = _cos;
combination->_sin = _sin;
_bgc_fp32_turn2_normalize(combination);
}
inline void bgc_fp64_turn2_combine(BGC_FP64_Turn2* const combination, const BGC_FP64_Turn2* const turn1, const BGC_FP64_Turn2* const turn2)
{
bgc_fp64_turn2_set_values(
combination,
turn1->_cos * turn2->_cos - turn1->_sin * turn2->_sin,
turn1->_cos * turn2->_sin + turn1->_sin * turn2->_cos
);
const double _cos = turn1->_cos * turn2->_cos - turn1->_sin * turn2->_sin;
const double _sin = turn1->_cos * turn2->_sin + turn1->_sin * turn2->_cos;
combination->_cos = _cos;
combination->_sin = _sin;
_bgc_fp64_turn2_normalize(combination);
}
// ============ Combination of three ============ //
inline void bgc_fp32_turn2_combine3(BGC_FP32_Turn2* const combination, const BGC_FP32_Turn2* const turn1, const BGC_FP32_Turn2* const turn2, const BGC_FP32_Turn2* const turn3)
{
const float _cos_of_two = turn1->_cos * turn2->_cos - turn1->_sin * turn2->_sin;
const float _sin_of_two = turn1->_cos * turn2->_sin + turn1->_sin * turn2->_cos;
const float _cos_of_three = _cos_of_two * turn3->_cos - _sin_of_two * turn3->_sin;
const float _sin_of_three = _cos_of_two * turn3->_sin + _sin_of_two * turn3->_cos;
combination->_cos = _cos_of_three;
combination->_sin = _sin_of_three;
_bgc_fp32_turn2_normalize(combination);
}
inline void bgc_fp64_turn2_combine3(BGC_FP64_Turn2* const combination, const BGC_FP64_Turn2* const turn1, const BGC_FP64_Turn2* const turn2, const BGC_FP64_Turn2* const turn3)
{
const double _cos_of_two = turn1->_cos * turn2->_cos - turn1->_sin * turn2->_sin;
const double _sin_of_two = turn1->_cos * turn2->_sin + turn1->_sin * turn2->_cos;
const double _cos_of_three = _cos_of_two * turn3->_cos - _sin_of_two * turn3->_sin;
const double _sin_of_three = _cos_of_two * turn3->_sin + _sin_of_two * turn3->_cos;
combination->_cos = _cos_of_three;
combination->_sin = _sin_of_three;
_bgc_fp64_turn2_normalize(combination);
}
// ================= Exclusion ================== //
inline void bgc_fp32_turn2_exclude(BGC_FP32_Turn2* const difference, const BGC_FP32_Turn2* const base, const BGC_FP32_Turn2* const excludant)
{
bgc_fp32_turn2_set_values(
difference,
base->_cos * excludant->_cos + base->_sin * excludant->_sin,
base->_sin * excludant->_cos - base->_cos * excludant->_sin
);
const float _cos = base->_cos * excludant->_cos + base->_sin * excludant->_sin;
const float _sin = base->_sin * excludant->_cos - base->_cos * excludant->_sin;
difference->_cos = _cos;
difference->_sin = _sin;
_bgc_fp32_turn2_normalize(difference);
}
inline void bgc_fp64_turn2_exclude(BGC_FP64_Turn2* const difference, const BGC_FP64_Turn2* const base, const BGC_FP64_Turn2* const excludant)
{
bgc_fp64_turn2_set_values(
difference,
base->_cos * excludant->_cos + base->_sin * excludant->_sin,
base->_sin * excludant->_cos - base->_cos * excludant->_sin
);
const double _cos = base->_cos * excludant->_cos + base->_sin * excludant->_sin;
const double _sin = base->_sin * excludant->_cos - base->_cos * excludant->_sin;
difference->_cos = _cos;
difference->_sin = _sin;
_bgc_fp64_turn2_normalize(difference);
}
// ============== Rotation Matrix =============== //

8
basic-geometry/turn3.h
View file

@ -204,12 +204,12 @@ inline void bgc_fp64_turn3_swap(BGC_FP64_Turn3* const turn1, BGC_FP64_Turn3* con
inline int bgc_fp32_turn3_is_idle(const BGC_FP32_Turn3* const turn)
{
return turn->_versor.x1 * turn->_versor.x1 + turn->_versor.x2 * turn->_versor.x2 + turn->_versor.x3 * turn->_versor.x3 <= BGC_FP32_SQUARE_EPSILON;
return bgc_fp32_quaternion_is_real(&turn->_versor);
}
inline int bgc_fp64_turn3_is_idle(const BGC_FP64_Turn3* const turn)
{
return turn->_versor.x1 * turn->_versor.x1 + turn->_versor.x2 * turn->_versor.x2 + turn->_versor.x3 * turn->_versor.x3 <= BGC_FP64_SQUARE_EPSILON;
return bgc_fp64_quaternion_is_real(&turn->_versor);
}
// ================== Convert =================== //
@ -333,7 +333,7 @@ inline void bgc_fp32_turn3_combine3(BGC_FP32_Turn3* const combination, const BGC
{
BGC_FP32_Quaternion product;
bgc_fp32_quaternion_multiply_by_quaternion(&product, &second->_versor, &first->_versor);
_bgc_fp32_restrict_quaternion_multiply_by_quaternion(&product, &second->_versor, &first->_versor);
bgc_fp32_quaternion_multiply_by_quaternion(&combination->_versor, &third->_versor, &product);
@ -344,7 +344,7 @@ inline void bgc_fp64_turn3_combine3(BGC_FP64_Turn3* const combination, const BGC
{
BGC_FP64_Quaternion product;
bgc_fp64_quaternion_multiply_by_quaternion(&product, &second->_versor, &first->_versor);
_bgc_fp64_restrict_quaternion_multiply_by_quaternion(&product, &second->_versor, &first->_versor);
bgc_fp64_quaternion_multiply_by_quaternion(&combination->_versor, &third->_versor, &product);