Изменение функций нормализации, переименование make функий в set values, добавление внутренних restrict функций, гарантирующих оптимизальное выполнение открытых функций, независимо от компилятора
This commit is contained in:
parent
e6ac9023ec
commit
6945c69ef2
20 changed files with 324 additions and 337 deletions
|
|
@ -1,5 +1,3 @@
|
|||
#include <math.h>
|
||||
|
||||
#include "./angle.h"
|
||||
#include "./vector3.h"
|
||||
#include "./turn3.h"
|
||||
|
|
@ -11,8 +9,11 @@ const BGC_FP64_Turn3 BGC_FP64_IDLE_TURN3 = {{ 1.0, 0.0, 0.0, 0.0 }};
|
|||
extern inline void bgc_fp32_turn3_reset(BGC_FP32_Turn3* const turn);
|
||||
extern inline void bgc_fp64_turn3_reset(BGC_FP64_Turn3* const turn);
|
||||
|
||||
extern 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);
|
||||
extern 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);
|
||||
extern inline void _bgc_fp32_turn3_normalize(BGC_FP32_Turn3* const turn);
|
||||
extern inline void _bgc_fp64_turn3_normalize(BGC_FP64_Turn3* const turn);
|
||||
|
||||
extern inline void bgc_fp32_turn3_set_values(BGC_FP32_Turn3* const turn, const float s0, const float x1, const float x2, const float x3);
|
||||
extern inline void bgc_fp64_turn3_set_values(BGC_FP64_Turn3* const turn, const double s0, const double x1, const double x2, const double x3);
|
||||
|
||||
extern inline void bgc_fp32_turn3_get_quaternion(BGC_FP32_Quaternion* const quaternion, const BGC_FP32_Turn3* const turn);
|
||||
extern inline void bgc_fp64_turn3_get_quaternion(BGC_FP64_Quaternion* const quaternion, const BGC_FP64_Turn3* const turn);
|
||||
|
|
@ -77,29 +78,6 @@ extern inline void bgc_fp64_turn3_vector_back(BGC_FP64_Vector3* const turned_vec
|
|||
extern inline int bgc_fp32_turn3_are_close(const BGC_FP32_Turn3* const turn1, const BGC_FP32_Turn3* const turn2);
|
||||
extern inline int bgc_fp64_turn3_are_close(const BGC_FP64_Turn3* const turn1, const BGC_FP64_Turn3* const turn2);
|
||||
|
||||
// ================= Normalize ================== //
|
||||
|
||||
void _bgc_fp32_turn3_normalize(BGC_FP32_Turn3* const turn, const float square_modulus)
|
||||
{
|
||||
if (square_modulus <= BGC_FP32_SQUARE_EPSILON || isnan(square_modulus)) {
|
||||
bgc_fp32_turn3_reset(turn);
|
||||
return;
|
||||
}
|
||||
|
||||
bgc_fp32_quaternion_multiply_by_real_number(&turn->_versor, &turn->_versor, sqrtf(1.0f / square_modulus));
|
||||
}
|
||||
|
||||
void _bgc_fp64_turn3_normalize(BGC_FP64_Turn3* const turn, const double square_modulus)
|
||||
{
|
||||
if (square_modulus <= BGC_FP64_SQUARE_EPSILON || isnan(square_modulus)) {
|
||||
bgc_fp64_turn3_reset(turn);
|
||||
return;
|
||||
}
|
||||
|
||||
bgc_fp64_quaternion_multiply_by_real_number(&turn->_versor, &turn->_versor, sqrt(1.0 / square_modulus));
|
||||
}
|
||||
|
||||
|
||||
// ================ Get Rotation ================ //
|
||||
|
||||
float bgc_fp32_turn3_get_rotation(BGC_FP32_Vector3* const axis, const BGC_FP32_Turn3* const turn, const int angle_unit)
|
||||
|
|
@ -164,13 +142,9 @@ void bgc_fp32_turn3_set_rotation(BGC_FP32_Turn3* const turn, const float x1, con
|
|||
|
||||
const float multiplier = sine / sqrtf(square_vector);
|
||||
|
||||
bgc_fp32_quaternion_make(&turn->_versor, cosf(half_angle), x1 * multiplier, x2 * multiplier, x3 * multiplier);
|
||||
bgc_fp32_quaternion_set_values(&turn->_versor, cosf(half_angle), x1 * multiplier, x2 * multiplier, x3 * multiplier);
|
||||
|
||||
const float square_modulus = bgc_fp32_quaternion_get_square_magnitude(&turn->_versor);
|
||||
|
||||
if (!bgc_fp32_is_square_unit(square_modulus)) {
|
||||
_bgc_fp32_turn3_normalize(turn, square_modulus);
|
||||
}
|
||||
_bgc_fp32_turn3_normalize(turn);
|
||||
}
|
||||
|
||||
void bgc_fp64_turn3_set_rotation(BGC_FP64_Turn3* const turn, const double x1, const double x2, const double x3, const double angle, const int angle_unit)
|
||||
|
|
@ -193,13 +167,9 @@ void bgc_fp64_turn3_set_rotation(BGC_FP64_Turn3* const turn, const double x1, co
|
|||
|
||||
const double multiplier = sine / sqrt(square_vector);
|
||||
|
||||
bgc_fp64_quaternion_make(&turn->_versor, cos(half_angle), x1 * multiplier, x2 * multiplier, x3 * multiplier);
|
||||
bgc_fp64_quaternion_set_values(&turn->_versor, cos(half_angle), x1 * multiplier, x2 * multiplier, x3 * multiplier);
|
||||
|
||||
const double square_modulus = bgc_fp64_quaternion_get_square_magnitude(&turn->_versor);
|
||||
|
||||
if (!bgc_fp64_is_square_unit(square_modulus)) {
|
||||
_bgc_fp64_turn3_normalize(turn, square_modulus);
|
||||
}
|
||||
_bgc_fp64_turn3_normalize(turn);
|
||||
}
|
||||
|
||||
// ========= Find Direction Difference ========== //
|
||||
|
|
@ -229,11 +199,8 @@ int bgc_fp32_turn3_find_direction_difference(BGC_FP32_Turn3* const turn, const B
|
|||
const float axis_square_modulus = bgc_fp32_vector3_get_squared_length(&axis);
|
||||
|
||||
if (axis_square_modulus <= BGC_FP32_SQUARE_EPSILON * square_product) {
|
||||
if (dot_product < 0.0f) {
|
||||
return BGC_ERROR_TURN3_VECTORS_OPPOSITE;
|
||||
}
|
||||
|
||||
return BGC_SUCCESS;
|
||||
bgc_fp32_turn3_reset(turn);
|
||||
return dot_product < 0.0f ? BGC_ERROR_TURN3_VECTORS_OPPOSITE : BGC_SUCCESS;
|
||||
}
|
||||
|
||||
const float axis_modulus = sqrtf(axis_square_modulus);
|
||||
|
|
@ -243,7 +210,7 @@ int bgc_fp32_turn3_find_direction_difference(BGC_FP32_Turn3* const turn, const B
|
|||
|
||||
const float vector_multiplier = sinf(angle) / axis_modulus;
|
||||
|
||||
bgc_fp32_turn3_set_raw_values(turn, cosf(angle), axis.x1 * vector_multiplier, axis.x2 * vector_multiplier, axis.x3 * vector_multiplier);
|
||||
bgc_fp32_turn3_set_values(turn, cosf(angle), axis.x1 * vector_multiplier, axis.x2 * vector_multiplier, axis.x3 * vector_multiplier);
|
||||
|
||||
return BGC_SUCCESS;
|
||||
}
|
||||
|
|
@ -274,11 +241,7 @@ int bgc_fp64_turn3_find_direction_difference(BGC_FP64_Turn3* const turn, const B
|
|||
|
||||
if (axis_square_modulus <= BGC_FP64_SQUARE_EPSILON * square_product) {
|
||||
bgc_fp64_turn3_reset(turn);
|
||||
if (dot_product < 0.0) {
|
||||
return BGC_ERROR_TURN3_VECTORS_OPPOSITE;
|
||||
}
|
||||
|
||||
return BGC_SUCCESS;
|
||||
return dot_product < 0.0 ? BGC_ERROR_TURN3_VECTORS_OPPOSITE : BGC_SUCCESS;
|
||||
}
|
||||
|
||||
const double axis_modulus = sqrt(axis_square_modulus);
|
||||
|
|
@ -288,7 +251,7 @@ int bgc_fp64_turn3_find_direction_difference(BGC_FP64_Turn3* const turn, const B
|
|||
|
||||
const double vector_multiplier = sin(angle) / axis_modulus;
|
||||
|
||||
bgc_fp64_turn3_set_raw_values(turn, cos(angle), axis.x1 * vector_multiplier, axis.x2 * vector_multiplier, axis.x3 * vector_multiplier);
|
||||
bgc_fp64_turn3_set_values(turn, cos(angle), axis.x1 * vector_multiplier, axis.x2 * vector_multiplier, axis.x3 * vector_multiplier);
|
||||
|
||||
return BGC_SUCCESS;
|
||||
}
|
||||
|
|
@ -477,11 +440,7 @@ int bgc_fp32_turn3_find_pair_difference(
|
|||
bgc_fp32_quaternion_multiply_by_quaternion(&turn->_versor, &q2, &q1);
|
||||
|
||||
// Making a final versor (a normalized quaternion)
|
||||
const float square_modulus = bgc_fp32_quaternion_get_square_magnitude(&turn->_versor);
|
||||
|
||||
if (!bgc_fp32_is_square_unit(square_modulus)) {
|
||||
_bgc_fp32_turn3_normalize(turn, square_modulus);
|
||||
}
|
||||
_bgc_fp32_turn3_normalize(turn);
|
||||
|
||||
return BGC_SUCCESS;
|
||||
}
|
||||
|
|
@ -524,11 +483,7 @@ int bgc_fp64_turn3_find_pair_difference(
|
|||
bgc_fp64_quaternion_multiply_by_quaternion(&turn->_versor, &q2, &q1);
|
||||
|
||||
// Making a final versor (a normalized quaternion)
|
||||
const double square_modulus = bgc_fp64_quaternion_get_square_magnitude(&turn->_versor);
|
||||
|
||||
if (!bgc_fp64_is_square_unit(square_modulus)) {
|
||||
_bgc_fp64_turn3_normalize(turn, square_modulus);
|
||||
}
|
||||
_bgc_fp64_turn3_normalize(turn);
|
||||
|
||||
return BGC_SUCCESS;
|
||||
}
|
||||
|
|
@ -550,7 +505,7 @@ void bgc_fp32_turn3_get_power(BGC_FP32_Turn3* const power, const BGC_FP32_Turn3*
|
|||
|
||||
const float multiplier = sinf(angle) / vector_modulus;
|
||||
|
||||
bgc_fp32_turn3_set_raw_values(power, cosf(angle), base->_versor.x1 * multiplier, base->_versor.x2 * multiplier, base->_versor.x3 * multiplier);
|
||||
bgc_fp32_turn3_set_values(power, cosf(angle), base->_versor.x1 * multiplier, base->_versor.x2 * multiplier, base->_versor.x3 * multiplier);
|
||||
}
|
||||
|
||||
void bgc_fp64_turn3_get_power(BGC_FP64_Turn3* const power, const BGC_FP64_Turn3* const base, const double exponent)
|
||||
|
|
@ -568,7 +523,7 @@ void bgc_fp64_turn3_get_power(BGC_FP64_Turn3* const power, const BGC_FP64_Turn3*
|
|||
|
||||
const double multiplier = sin(angle) / vector_modulus;
|
||||
|
||||
bgc_fp64_turn3_set_raw_values(power, cos(angle), base->_versor.x1 * multiplier, base->_versor.x2 * multiplier, base->_versor.x3 * multiplier);
|
||||
bgc_fp64_turn3_set_values(power, cos(angle), base->_versor.x1 * multiplier, base->_versor.x2 * multiplier, base->_versor.x3 * multiplier);
|
||||
}
|
||||
|
||||
// ============ Sphere Interpolation ============ //
|
||||
|
|
@ -599,7 +554,7 @@ void bgc_fp32_turn3_spherically_interpolate(BGC_FP32_Turn3* const interpolation,
|
|||
const float turn_x3 = delta_x3 * multiplier;
|
||||
|
||||
// Combining of starting orientation with the turning
|
||||
bgc_fp32_turn3_set_raw_values(
|
||||
bgc_fp32_turn3_set_values(
|
||||
interpolation,
|
||||
(turn_s0 * start->_versor.s0 - turn_x1 * start->_versor.x1) - (turn_x2 * start->_versor.x2 + turn_x3 * start->_versor.x3),
|
||||
(turn_x1 * start->_versor.s0 + turn_s0 * start->_versor.x1) - (turn_x3 * start->_versor.x2 - turn_x2 * start->_versor.x3),
|
||||
|
|
@ -634,7 +589,7 @@ void bgc_fp64_turn3_spherically_interpolate(BGC_FP64_Turn3* const interpolation,
|
|||
const double turn_x3 = delta_x3 * multiplier;
|
||||
|
||||
// Combining of starting orientation with the turning
|
||||
bgc_fp64_turn3_set_raw_values(
|
||||
bgc_fp64_turn3_set_values(
|
||||
interpolation,
|
||||
(turn_s0 * start->_versor.s0 - turn_x1 * start->_versor.x1) - (turn_x2 * start->_versor.x2 + turn_x3 * start->_versor.x3),
|
||||
(turn_x1 * start->_versor.s0 + turn_s0 * start->_versor.x1) - (turn_x3 * start->_versor.x2 - turn_x2 * start->_versor.x3),
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue