Изменение функций нормализации, переименование make функий в set values, добавление внутренних restrict функций, гарантирующих оптимизальное выполнение открытых функций, независимо от компилятора
This commit is contained in:
parent
e6ac9023ec
commit
6945c69ef2
20 changed files with 324 additions and 337 deletions
|
|
@ -2,6 +2,7 @@
|
|||
#define _BGC_TURN3_H_INCLUDED_
|
||||
|
||||
#include <stdint.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "./utilities.h"
|
||||
#include "./types.h"
|
||||
|
|
@ -35,42 +36,82 @@ extern const BGC_FP64_Turn3 BGC_FP64_IDLE_TURN3;
|
|||
|
||||
inline void bgc_fp32_turn3_reset(BGC_FP32_Turn3* const turn)
|
||||
{
|
||||
bgc_fp32_quaternion_make(&turn->_versor, 1.0f, 0.0f, 0.0f, 0.0f);
|
||||
turn->_versor.s0 = 1.0f;
|
||||
turn->_versor.x1 = 0.0f;
|
||||
turn->_versor.x2 = 0.0f;
|
||||
turn->_versor.x3 = 0.0f;
|
||||
}
|
||||
|
||||
inline void bgc_fp64_turn3_reset(BGC_FP64_Turn3* const turn)
|
||||
{
|
||||
bgc_fp64_quaternion_make(&turn->_versor, 1.0, 0.0, 0.0, 0.0);
|
||||
turn->_versor.s0 = 1.0;
|
||||
turn->_versor.x1 = 0.0;
|
||||
turn->_versor.x2 = 0.0;
|
||||
turn->_versor.x3 = 0.0;
|
||||
}
|
||||
|
||||
// ============= Private: Normalize ============= //
|
||||
|
||||
void _bgc_fp32_turn3_normalize(BGC_FP32_Turn3* const turn, const float square_modulus);
|
||||
inline void _bgc_fp32_turn3_normalize(BGC_FP32_Turn3* const turn)
|
||||
{
|
||||
const float square_magnitude = bgc_fp32_quaternion_get_square_magnitude(&turn->_versor);
|
||||
|
||||
void _bgc_fp64_turn3_normalize(BGC_FP64_Turn3* const turn, const double square_modulus);
|
||||
if (bgc_fp32_is_square_unit(square_magnitude)) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (square_magnitude <= BGC_FP32_SQUARE_EPSILON || isnan(square_magnitude)) {
|
||||
turn->_versor.s0 = 1.0f;
|
||||
turn->_versor.x1 = 0.0f;
|
||||
turn->_versor.x2 = 0.0f;
|
||||
turn->_versor.x3 = 0.0f;
|
||||
return;
|
||||
}
|
||||
|
||||
const float multiplier = sqrtf(1.0f / square_magnitude);
|
||||
|
||||
turn->_versor.s0 *= multiplier;
|
||||
turn->_versor.x1 *= multiplier;
|
||||
turn->_versor.x2 *= multiplier;
|
||||
turn->_versor.x3 *= multiplier;
|
||||
}
|
||||
|
||||
inline void _bgc_fp64_turn3_normalize(BGC_FP64_Turn3* const turn)
|
||||
{
|
||||
const double square_magnitude = bgc_fp64_quaternion_get_square_magnitude(&turn->_versor);
|
||||
|
||||
if (bgc_fp64_is_square_unit(square_magnitude)) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (square_magnitude <= BGC_FP64_SQUARE_EPSILON || isnan(square_magnitude)) {
|
||||
turn->_versor.s0 = 1.0;
|
||||
turn->_versor.x1 = 0.0;
|
||||
turn->_versor.x2 = 0.0;
|
||||
turn->_versor.x3 = 0.0;
|
||||
return;
|
||||
}
|
||||
|
||||
const double multiplier = sqrt(1.0 / square_magnitude);
|
||||
|
||||
turn->_versor.s0 *= multiplier;
|
||||
turn->_versor.x1 *= multiplier;
|
||||
turn->_versor.x2 *= multiplier;
|
||||
turn->_versor.x3 *= multiplier;
|
||||
}
|
||||
|
||||
// ================= Set Values ================= //
|
||||
|
||||
inline void bgc_fp32_turn3_set_raw_values(BGC_FP32_Turn3* const turn, const float s0, const float x1, const float x2, const float x3)
|
||||
inline void bgc_fp32_turn3_set_values(BGC_FP32_Turn3* const turn, const float s0, const float x1, const float x2, const float x3)
|
||||
{
|
||||
bgc_fp32_quaternion_make(&turn->_versor, s0, x1, x2, x3);
|
||||
|
||||
const float square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
|
||||
|
||||
if (!bgc_fp32_is_square_unit(square_modulus)) {
|
||||
_bgc_fp32_turn3_normalize(turn, square_modulus);
|
||||
}
|
||||
bgc_fp32_quaternion_set_values(&turn->_versor, s0, x1, x2, x3);
|
||||
_bgc_fp32_turn3_normalize(turn);
|
||||
}
|
||||
|
||||
inline void bgc_fp64_turn3_set_raw_values(BGC_FP64_Turn3* const turn, const double s0, const double x1, const double x2, const double x3)
|
||||
inline void bgc_fp64_turn3_set_values(BGC_FP64_Turn3* const turn, const double s0, const double x1, const double x2, const double x3)
|
||||
{
|
||||
bgc_fp64_quaternion_make(&turn->_versor, s0, x1, x2, x3);
|
||||
|
||||
const double square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
|
||||
|
||||
if (!bgc_fp64_is_square_unit(square_modulus)) {
|
||||
_bgc_fp64_turn3_normalize(turn, square_modulus);
|
||||
}
|
||||
bgc_fp64_quaternion_set_values(&turn->_versor, s0, x1, x2, x3);
|
||||
_bgc_fp64_turn3_normalize(turn);
|
||||
}
|
||||
|
||||
// =============== Get Quaternion =============== //
|
||||
|
|
@ -90,23 +131,13 @@ inline void bgc_fp64_turn3_get_quaternion(BGC_FP64_Quaternion* const quaternion,
|
|||
inline void bgc_fp32_turn3_set_quaternion(BGC_FP32_Turn3* const turn, const BGC_FP32_Quaternion* const quaternion)
|
||||
{
|
||||
bgc_fp32_quaternion_copy(&turn->_versor, quaternion);
|
||||
|
||||
const float square_modulus = bgc_fp32_quaternion_get_square_magnitude(quaternion);
|
||||
|
||||
if (!bgc_fp32_is_square_unit(square_modulus)) {
|
||||
_bgc_fp32_turn3_normalize(turn, square_modulus);
|
||||
}
|
||||
_bgc_fp32_turn3_normalize(turn);
|
||||
}
|
||||
|
||||
inline void bgc_fp64_turn3_set_quaternion(BGC_FP64_Turn3* const turn, const BGC_FP64_Quaternion* const quaternion)
|
||||
{
|
||||
bgc_fp64_quaternion_copy(&turn->_versor, quaternion);
|
||||
|
||||
const double square_modulus = bgc_fp64_quaternion_get_square_magnitude(quaternion);
|
||||
|
||||
if (!bgc_fp64_is_square_unit(square_modulus)) {
|
||||
_bgc_fp64_turn3_normalize(turn, square_modulus);
|
||||
}
|
||||
_bgc_fp64_turn3_normalize(turn);
|
||||
}
|
||||
|
||||
// ================ Get Rotation ================ //
|
||||
|
|
@ -186,23 +217,14 @@ inline int bgc_fp64_turn3_is_idle(const BGC_FP64_Turn3* const turn)
|
|||
inline void bgc_fp32_turn3_convert_to_fp64(BGC_FP64_Turn3* const destination, const BGC_FP32_Turn3* const source)
|
||||
{
|
||||
bgc_fp32_quaternion_convert_to_fp64(&destination->_versor, &source->_versor);
|
||||
_bgc_fp64_turn3_normalize(destination);
|
||||
|
||||
const double square_modulus = bgc_fp64_quaternion_get_square_magnitude(&destination->_versor);
|
||||
|
||||
if (!bgc_fp64_is_square_unit(square_modulus)) {
|
||||
_bgc_fp64_turn3_normalize(destination, square_modulus);
|
||||
}
|
||||
}
|
||||
|
||||
inline void bgc_fp64_turn3_convert_to_fp32(BGC_FP32_Turn3* const destination, const BGC_FP64_Turn3* const source)
|
||||
{
|
||||
bgc_fp64_quaternion_convert_to_fp32(&destination->_versor, &source->_versor);
|
||||
|
||||
const float square_modulus = bgc_fp32_quaternion_get_square_magnitude(&destination->_versor);
|
||||
|
||||
if (!bgc_fp32_is_square_unit(square_modulus)) {
|
||||
_bgc_fp32_turn3_normalize(destination, square_modulus);
|
||||
}
|
||||
_bgc_fp32_turn3_normalize(destination);
|
||||
}
|
||||
|
||||
// ================== Shorten =================== //
|
||||
|
|
@ -296,23 +318,13 @@ void bgc_fp64_turn3_get_power(BGC_FP64_Turn3* const power, const BGC_FP64_Turn3*
|
|||
inline void bgc_fp32_turn3_combine(BGC_FP32_Turn3* const combination, const BGC_FP32_Turn3* const first, const BGC_FP32_Turn3* const second)
|
||||
{
|
||||
bgc_fp32_quaternion_multiply_by_quaternion(&combination->_versor, &second->_versor, &first->_versor);
|
||||
|
||||
const float square_modulus = bgc_fp32_quaternion_get_square_magnitude(&combination->_versor);
|
||||
|
||||
if (!bgc_fp32_is_square_unit(square_modulus)) {
|
||||
_bgc_fp32_turn3_normalize(combination, square_modulus);
|
||||
}
|
||||
_bgc_fp32_turn3_normalize(combination);
|
||||
}
|
||||
|
||||
inline void bgc_fp64_turn3_combine(BGC_FP64_Turn3* const combination, const BGC_FP64_Turn3* const first, const BGC_FP64_Turn3* const second)
|
||||
{
|
||||
bgc_fp64_quaternion_multiply_by_quaternion(&combination->_versor, &second->_versor, &first->_versor);
|
||||
|
||||
const double square_modulus = bgc_fp64_quaternion_get_square_magnitude(&combination->_versor);
|
||||
|
||||
if (!bgc_fp64_is_square_unit(square_modulus)) {
|
||||
_bgc_fp64_turn3_normalize(combination, square_modulus);
|
||||
}
|
||||
_bgc_fp64_turn3_normalize(combination);
|
||||
}
|
||||
|
||||
// ============ Combination of three ============ //
|
||||
|
|
@ -325,11 +337,7 @@ inline void bgc_fp32_turn3_combine3(BGC_FP32_Turn3* const combination, const BGC
|
|||
|
||||
bgc_fp32_quaternion_multiply_by_quaternion(&combination->_versor, &third->_versor, &product);
|
||||
|
||||
const float square_modulus = bgc_fp32_quaternion_get_square_magnitude(&combination->_versor);
|
||||
|
||||
if (!bgc_fp32_is_square_unit(square_modulus)) {
|
||||
_bgc_fp32_turn3_normalize(combination, square_modulus);
|
||||
}
|
||||
_bgc_fp32_turn3_normalize(combination);
|
||||
}
|
||||
|
||||
inline void bgc_fp64_turn3_combine3(BGC_FP64_Turn3* const combination, const BGC_FP64_Turn3* const first, const BGC_FP64_Turn3* const second, const BGC_FP64_Turn3* const third)
|
||||
|
|
@ -340,11 +348,7 @@ inline void bgc_fp64_turn3_combine3(BGC_FP64_Turn3* const combination, const BGC
|
|||
|
||||
bgc_fp64_quaternion_multiply_by_quaternion(&combination->_versor, &third->_versor, &product);
|
||||
|
||||
const double square_modulus = bgc_fp64_quaternion_get_square_magnitude(&combination->_versor);
|
||||
|
||||
if (!bgc_fp64_is_square_unit(square_modulus)) {
|
||||
_bgc_fp64_turn3_normalize(combination, square_modulus);
|
||||
}
|
||||
_bgc_fp64_turn3_normalize(combination);
|
||||
}
|
||||
|
||||
// ================= Exclusion ================== //
|
||||
|
|
@ -353,22 +357,14 @@ inline void bgc_fp32_turn3_exclude(BGC_FP32_Turn3* const difference, const BGC_F
|
|||
{
|
||||
bgc_fp32_quaternion_multiply_by_conjugate(&difference->_versor, &base->_versor, &excludant->_versor);
|
||||
|
||||
const float square_modulus = bgc_fp32_quaternion_get_square_magnitude(&difference->_versor);
|
||||
|
||||
if (!bgc_fp32_is_square_unit(square_modulus)) {
|
||||
_bgc_fp32_turn3_normalize(difference, square_modulus);
|
||||
}
|
||||
_bgc_fp32_turn3_normalize(difference);
|
||||
}
|
||||
|
||||
inline void bgc_fp64_turn3_exclude(BGC_FP64_Turn3* const difference, const BGC_FP64_Turn3* const base, const BGC_FP64_Turn3* const excludant)
|
||||
{
|
||||
bgc_fp64_quaternion_multiply_by_conjugate(&difference->_versor, &base->_versor, &excludant->_versor);
|
||||
|
||||
const double square_modulus = bgc_fp64_quaternion_get_square_magnitude(&difference->_versor);
|
||||
|
||||
if (!bgc_fp64_is_square_unit(square_modulus)) {
|
||||
_bgc_fp64_turn3_normalize(difference, square_modulus);
|
||||
}
|
||||
_bgc_fp64_turn3_normalize(difference);
|
||||
}
|
||||
|
||||
// ============ Sphere Interpolation ============ //
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue