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

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 =============== //