604 lines
26 KiB
C
604 lines
26 KiB
C
#include "./angle.h"
|
|
#include "./vector3.h"
|
|
#include "./turn3.h"
|
|
|
|
const BGC_FP32_Turn3 BGC_FP32_IDLE_TURN3 = {{ 1.0f, 0.0f, 0.0f, 0.0f }};
|
|
|
|
const BGC_FP64_Turn3 BGC_FP64_IDLE_TURN3 = {{ 1.0, 0.0, 0.0, 0.0 }};
|
|
|
|
int64_t turn3_normalize_counter = 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_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 s, const float x, const float y, const float z);
|
|
extern inline void bgc_fp64_turn3_set_values(BGC_FP64_Turn3* const turn, const double s, const double x, const double y, const double z);
|
|
|
|
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);
|
|
|
|
extern inline void bgc_fp32_turn3_set_quaternion(BGC_FP32_Turn3* const turn, const BGC_FP32_Quaternion* const quaternion);
|
|
extern inline void bgc_fp64_turn3_set_quaternion(BGC_FP64_Turn3* const turn, const BGC_FP64_Quaternion* const quaternion);
|
|
|
|
extern inline void bgc_fp32_turn3_copy(BGC_FP32_Turn3* const destination, const BGC_FP32_Turn3* const source);
|
|
extern inline void bgc_fp64_turn3_copy(BGC_FP64_Turn3* const destination, const BGC_FP64_Turn3* const source);
|
|
|
|
extern inline void bgc_fp32_turn3_swap(BGC_FP32_Turn3* const turn1, BGC_FP32_Turn3* const turn2);
|
|
extern inline void bgc_fp64_turn3_swap(BGC_FP64_Turn3* const turn1, BGC_FP64_Turn3* const turn2);
|
|
|
|
extern inline int bgc_fp32_turn3_is_idle(const BGC_FP32_Turn3* const turn);
|
|
extern inline int bgc_fp64_turn3_is_idle(const BGC_FP64_Turn3* const turn);
|
|
|
|
extern inline void bgc_fp32_turn3_convert_to_fp64(BGC_FP64_Turn3* const destination, const BGC_FP32_Turn3* const source);
|
|
extern inline void bgc_fp64_turn3_convert_to_fp32(BGC_FP32_Turn3* const destination, const BGC_FP64_Turn3* const source);
|
|
|
|
extern inline void bgc_fp32_turn3_shorten(BGC_FP32_Turn3* const turn);
|
|
extern inline void bgc_fp64_turn3_shorten(BGC_FP64_Turn3* const turn);
|
|
|
|
extern inline void bgc_fp32_turn3_get_shortened(BGC_FP32_Turn3* const shortened, const BGC_FP32_Turn3* const turn);
|
|
extern inline void bgc_fp64_turn3_get_shortened(BGC_FP64_Turn3* const shortened, const BGC_FP64_Turn3* const turn);
|
|
|
|
extern inline void bgc_fp32_turn3_alternate(BGC_FP32_Turn3* const turn);
|
|
extern inline void bgc_fp64_turn3_alternate(BGC_FP64_Turn3* const turn);
|
|
|
|
extern inline void bgc_fp32_turn3_get_alternative(BGC_FP32_Turn3* const alternative, const BGC_FP32_Turn3* const turn);
|
|
extern inline void bgc_fp64_turn3_get_alternative(BGC_FP64_Turn3* const alternative, const BGC_FP64_Turn3* const turn);
|
|
|
|
extern inline void bgc_fp32_turn3_revert(BGC_FP32_Turn3* const turn);
|
|
extern inline void bgc_fp64_turn3_revert(BGC_FP64_Turn3* const turn);
|
|
|
|
extern inline void bgc_fp32_turn3_get_reverse(BGC_FP32_Turn3* const inverse, const BGC_FP32_Turn3* const turn);
|
|
extern inline void bgc_fp64_turn3_get_reverse(BGC_FP64_Turn3* const inverse, const BGC_FP64_Turn3* const turn);
|
|
|
|
extern inline void bgc_fp32_turn3_combine(BGC_FP32_Turn3* const combination, const BGC_FP32_Turn3* const external_turn, const BGC_FP32_Turn3* const internal_turn);
|
|
extern inline void bgc_fp64_turn3_combine(BGC_FP64_Turn3* const combination, const BGC_FP64_Turn3* const external_turn, const BGC_FP64_Turn3* const internal_turn);
|
|
|
|
extern inline void bgc_fp32_turn3_exclude(BGC_FP32_Turn3* const difference, const BGC_FP32_Turn3* const turn, const BGC_FP32_Turn3* const excludant);
|
|
extern inline void bgc_fp64_turn3_exclude(BGC_FP64_Turn3* const difference, const BGC_FP64_Turn3* const turn, const BGC_FP64_Turn3* const excludant);
|
|
|
|
extern inline void bgc_fp32_turn3_get_rotation_matrix(BGC_FP32_Matrix3x3* const matrix, const BGC_FP32_Turn3* const turn);
|
|
extern inline void bgc_fp64_turn3_get_rotation_matrix(BGC_FP64_Matrix3x3* const matrix, const BGC_FP64_Turn3* const turn);
|
|
|
|
extern inline void bgc_fp32_turn3_get_reverse_matrix(BGC_FP32_Matrix3x3* const matrix, const BGC_FP32_Turn3* const turn);
|
|
extern inline void bgc_fp64_turn3_get_reverse_matrix(BGC_FP64_Matrix3x3* const matrix, const BGC_FP64_Turn3* const turn);
|
|
|
|
extern inline void bgc_fp32_turn3_get_both_matrices(BGC_FP32_Matrix3x3* const rotation, BGC_FP32_Matrix3x3* const reverse, const BGC_FP32_Turn3* const turn);
|
|
extern inline void bgc_fp64_turn3_get_both_matrices(BGC_FP64_Matrix3x3* const rotation, BGC_FP64_Matrix3x3* const reverse, const BGC_FP64_Turn3* const turn);
|
|
|
|
extern inline void bgc_fp32_turn3_vector(BGC_FP32_Vector3* const turned_vector, const BGC_FP32_Turn3* const turn, const BGC_FP32_Vector3* const vector);
|
|
extern inline void bgc_fp64_turn3_vector(BGC_FP64_Vector3* const turned_vector, const BGC_FP64_Turn3* const turn, const BGC_FP64_Vector3* const vector);
|
|
|
|
extern inline void bgc_fp32_turn3_vector_back(BGC_FP32_Vector3* const turned_vector, const BGC_FP32_Turn3* const turn, const BGC_FP32_Vector3* const vector);
|
|
extern inline void bgc_fp64_turn3_vector_back(BGC_FP64_Vector3* const turned_vector, const BGC_FP64_Turn3* const turn, const BGC_FP64_Vector3* const vector);
|
|
|
|
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);
|
|
|
|
// ================ Get Rotation ================ //
|
|
|
|
float bgc_fp32_turn3_get_rotation(BGC_FP32_Vector3* const axis, const BGC_FP32_Turn3* const turn, const int angle_unit)
|
|
{
|
|
const float square_vector_modulus = turn->_versor.x * turn->_versor.x + turn->_versor.y * turn->_versor.y + turn->_versor.z * turn->_versor.z;
|
|
|
|
if (square_vector_modulus <= BGC_FP32_SQUARE_EPSILON) {
|
|
bgc_fp32_vector3_reset(axis);
|
|
return 0.0f;
|
|
}
|
|
|
|
const float vector_modulus = sqrtf(square_vector_modulus);
|
|
|
|
const float multiplier = 1.0f / vector_modulus;
|
|
|
|
axis->x = turn->_versor.x * multiplier;
|
|
axis->y = turn->_versor.y * multiplier;
|
|
axis->z = turn->_versor.z * multiplier;
|
|
|
|
return 2.0f * atan2f(vector_modulus, turn->_versor.s);
|
|
}
|
|
|
|
double bgc_fp64_turn3_get_rotation(BGC_FP64_Vector3* const axis, const BGC_FP64_Turn3* const turn, const int angle_unit)
|
|
{
|
|
const double square_vector_modulus = turn->_versor.x * turn->_versor.x + turn->_versor.y * turn->_versor.y + turn->_versor.z * turn->_versor.z;
|
|
|
|
if (square_vector_modulus <= BGC_FP64_SQUARE_EPSILON) {
|
|
bgc_fp64_vector3_reset(axis);
|
|
return 0.0;
|
|
}
|
|
|
|
const double vector_modulus = sqrt(square_vector_modulus);
|
|
|
|
const double multiplier = 1.0 / vector_modulus;
|
|
|
|
axis->x = turn->_versor.x * multiplier;
|
|
axis->y = turn->_versor.y * multiplier;
|
|
axis->z = turn->_versor.z * multiplier;
|
|
|
|
return 2.0 * atan2(vector_modulus, turn->_versor.s);
|
|
}
|
|
|
|
// ================ Set Rotation ================ //
|
|
|
|
void bgc_fp32_turn3_set_rotation(BGC_FP32_Turn3* const turn, const float x, const float y, const float z, const float angle, const int angle_unit)
|
|
{
|
|
const float square_vector = x * x + y * y + z * z;
|
|
|
|
if (square_vector <= BGC_FP32_SQUARE_EPSILON) {
|
|
bgc_fp32_turn3_reset(turn);
|
|
return;
|
|
}
|
|
|
|
const float half_angle = bgc_fp32_angle_to_radians(0.5f * angle, angle_unit);
|
|
|
|
const float sine = sinf(half_angle);
|
|
|
|
if (bgc_fp32_is_zero(sine)) {
|
|
bgc_fp32_turn3_reset(turn);
|
|
return;
|
|
}
|
|
|
|
const float multiplier = sine / sqrtf(square_vector);
|
|
|
|
turn->_versor.s = cosf(half_angle);
|
|
turn->_versor.x = x * multiplier;
|
|
turn->_versor.y = y * multiplier;
|
|
turn->_versor.z = z * multiplier;
|
|
|
|
_bgc_fp32_turn3_normalize(turn);
|
|
}
|
|
|
|
void bgc_fp64_turn3_set_rotation(BGC_FP64_Turn3* const turn, const double x, const double y, const double z, const double angle, const int angle_unit)
|
|
{
|
|
const double square_vector = x * x + y * y + z * z;
|
|
|
|
if (square_vector <= BGC_FP64_SQUARE_EPSILON) {
|
|
bgc_fp64_turn3_reset(turn);
|
|
return;
|
|
}
|
|
|
|
const double half_angle = bgc_fp64_angle_to_radians(0.5 * angle, angle_unit);
|
|
|
|
const double sine = sin(half_angle);
|
|
|
|
if (bgc_fp64_is_zero(sine)) {
|
|
bgc_fp64_turn3_reset(turn);
|
|
return;
|
|
}
|
|
|
|
const double multiplier = sine / sqrt(square_vector);
|
|
|
|
turn->_versor.s = cos(half_angle);
|
|
turn->_versor.x = x * multiplier;
|
|
turn->_versor.y = y * multiplier;
|
|
turn->_versor.z = z * multiplier;
|
|
|
|
_bgc_fp64_turn3_normalize(turn);
|
|
}
|
|
|
|
// ========= Find Direction Difference ========== //
|
|
|
|
int bgc_fp32_turn3_find_direction_difference(BGC_FP32_Turn3* const turn, const BGC_FP32_Vector3* const first, const BGC_FP32_Vector3* const second)
|
|
{
|
|
const float first_square_modulus = bgc_fp32_vector3_get_squared_length(first);
|
|
|
|
bgc_fp32_turn3_reset(turn);
|
|
|
|
if (first_square_modulus <= BGC_FP32_SQUARE_EPSILON) {
|
|
return BGC_ERROR_TURN3_FIRST_VECTOR_ZERO;
|
|
}
|
|
|
|
const float second_square_modulus = bgc_fp32_vector3_get_squared_length(second);
|
|
|
|
if (second_square_modulus <= BGC_FP32_SQUARE_EPSILON) {
|
|
return BGC_ERROR_TURN3_SECOND_VECTOR_ZERO;
|
|
}
|
|
|
|
BGC_FP32_Vector3 axis;
|
|
|
|
bgc_fp32_vector3_get_cross_product(&axis, first, second);
|
|
|
|
const float square_product = first_square_modulus * second_square_modulus;
|
|
const float dot_product = bgc_fp32_vector3_get_dot_product(first, second);
|
|
const float axis_square_modulus = bgc_fp32_vector3_get_squared_length(&axis);
|
|
|
|
if (axis_square_modulus <= BGC_FP32_SQUARE_EPSILON * square_product) {
|
|
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);
|
|
const float trigonometry_fix = sqrtf(1.0f / square_product);
|
|
|
|
const float angle = 0.5f * atan2f(axis_modulus * trigonometry_fix, dot_product * trigonometry_fix);
|
|
|
|
const float vector_multiplier = sinf(angle) / axis_modulus;
|
|
|
|
bgc_fp32_turn3_set_values(turn, cosf(angle), axis.x * vector_multiplier, axis.y * vector_multiplier, axis.z * vector_multiplier);
|
|
|
|
return BGC_SUCCESS;
|
|
}
|
|
|
|
int bgc_fp64_turn3_find_direction_difference(BGC_FP64_Turn3* const turn, const BGC_FP64_Vector3* const first, const BGC_FP64_Vector3* const second)
|
|
{
|
|
const double first_square_modulus = bgc_fp64_vector3_get_squared_length(first);
|
|
|
|
bgc_fp64_turn3_reset(turn);
|
|
|
|
if (first_square_modulus <= BGC_FP64_SQUARE_EPSILON) {
|
|
return BGC_ERROR_TURN3_FIRST_VECTOR_ZERO;
|
|
}
|
|
|
|
const double second_square_modulus = bgc_fp64_vector3_get_squared_length(second);
|
|
|
|
if (second_square_modulus <= BGC_FP64_SQUARE_EPSILON) {
|
|
return BGC_ERROR_TURN3_SECOND_VECTOR_ZERO;
|
|
}
|
|
|
|
BGC_FP64_Vector3 axis;
|
|
|
|
bgc_fp64_vector3_get_cross_product(&axis, first, second);
|
|
|
|
const double square_product = first_square_modulus * second_square_modulus;
|
|
const double dot_product = bgc_fp64_vector3_get_dot_product(first, second);
|
|
const double axis_square_modulus = bgc_fp64_vector3_get_squared_length(&axis);
|
|
|
|
if (axis_square_modulus <= BGC_FP64_SQUARE_EPSILON * square_product) {
|
|
bgc_fp64_turn3_reset(turn);
|
|
return dot_product < 0.0 ? BGC_ERROR_TURN3_VECTORS_OPPOSITE : BGC_SUCCESS;
|
|
}
|
|
|
|
const double axis_modulus = sqrt(axis_square_modulus);
|
|
const double trigonometry_fix = sqrt(1.0 / square_product);
|
|
|
|
const double angle = 0.5 * atan2(axis_modulus * trigonometry_fix, dot_product * trigonometry_fix);
|
|
|
|
const double vector_multiplier = sin(angle) / axis_modulus;
|
|
|
|
bgc_fp64_turn3_set_values(turn, cos(angle), axis.x * vector_multiplier, axis.y * vector_multiplier, axis.z * vector_multiplier);
|
|
|
|
return BGC_SUCCESS;
|
|
}
|
|
|
|
// ============ Make Orthogonal Pair ============ //
|
|
|
|
static inline int _bgc_fp32_turn3_get_orthogonal_pair(BGC_FP32_Vector3* const unit_main, BGC_FP32_Vector3* const unit_branch, const BGC_FP32_Vector3* const main, const BGC_FP32_Vector3* const branch)
|
|
{
|
|
const float main_square_modulus = bgc_fp32_vector3_get_squared_length(main);
|
|
|
|
if (main_square_modulus <= BGC_FP32_SQUARE_EPSILON) {
|
|
return _BGC_ERROR_TURN3_EMPTY_MAIN;
|
|
}
|
|
|
|
const float branch_square_modulus = bgc_fp32_vector3_get_squared_length(branch);
|
|
|
|
if (branch_square_modulus <= BGC_FP32_SQUARE_EPSILON) {
|
|
return _BGC_ERROR_TURN3_EMPTY_BRANCH;
|
|
}
|
|
|
|
bgc_fp32_vector3_multiply_by_real_number(unit_main, main, sqrtf(1.0f / main_square_modulus));
|
|
|
|
bgc_fp32_vector3_add_scaled(unit_branch, branch, unit_main, -bgc_fp32_vector3_get_dot_product(branch, unit_main));
|
|
|
|
const float orthogonal_square_modulus = bgc_fp32_vector3_get_squared_length(unit_branch);
|
|
|
|
if (orthogonal_square_modulus <= BGC_FP32_SQUARE_EPSILON) {
|
|
return _BGC_ERROR_TURN3_PAIR_PARALLEL;
|
|
}
|
|
|
|
bgc_fp32_vector3_multiply_by_real_number(unit_branch, unit_branch, sqrtf(1.0f / orthogonal_square_modulus));
|
|
|
|
return BGC_SUCCESS;
|
|
}
|
|
|
|
static inline int _bgc_fp64_turn3_get_orthogonal_pair(BGC_FP64_Vector3* const unit_main, BGC_FP64_Vector3* const unit_branch, const BGC_FP64_Vector3* const main, const BGC_FP64_Vector3* const branch)
|
|
{
|
|
const double main_square_modulus = bgc_fp64_vector3_get_squared_length(main);
|
|
|
|
if (main_square_modulus <= BGC_FP64_SQUARE_EPSILON) {
|
|
return _BGC_ERROR_TURN3_EMPTY_MAIN;
|
|
}
|
|
|
|
const double branch_square_modulus = bgc_fp64_vector3_get_squared_length(branch);
|
|
|
|
if (branch_square_modulus <= BGC_FP64_SQUARE_EPSILON) {
|
|
return _BGC_ERROR_TURN3_EMPTY_BRANCH;
|
|
}
|
|
|
|
bgc_fp64_vector3_multiply_by_real_number(unit_main, main, sqrt(1.0 / main_square_modulus));
|
|
|
|
bgc_fp64_vector3_add_scaled(unit_branch, branch, unit_main, -bgc_fp64_vector3_get_dot_product(branch, unit_main));
|
|
|
|
const double orthogonal_square_modulus = bgc_fp64_vector3_get_squared_length(unit_branch);
|
|
|
|
if (orthogonal_square_modulus <= BGC_FP64_SQUARE_EPSILON) {
|
|
return _BGC_ERROR_TURN3_PAIR_PARALLEL;
|
|
}
|
|
|
|
bgc_fp64_vector3_multiply_by_real_number(unit_branch, unit_branch, sqrt(1.0 / orthogonal_square_modulus));
|
|
|
|
return BGC_SUCCESS;
|
|
}
|
|
|
|
// ========= Make Direction Difference ========== //
|
|
|
|
static inline void _bgc_fp32_turn3_get_turning_quaternion(BGC_FP32_Quaternion* const quaternion, const BGC_FP32_Vector3* const unit_start, const BGC_FP32_Vector3* const unit_end, const BGC_FP32_Vector3* const unit_orthogonal)
|
|
{
|
|
BGC_FP32_Vector3 axis;
|
|
|
|
bgc_fp32_vector3_get_cross_product(&axis, unit_start, unit_end);
|
|
|
|
const float dot_product = bgc_fp32_vector3_get_dot_product(unit_start, unit_end);
|
|
|
|
const float axis_square_modulus = bgc_fp32_vector3_get_squared_length(&axis);
|
|
|
|
// unit_start and unit_end are parallel
|
|
if (axis_square_modulus <= BGC_FP32_SQUARE_EPSILON) {
|
|
// unit_start and unit_end are co-directional, angle = 180 degrees
|
|
if (dot_product >= 0.0f) {
|
|
quaternion->s = 1.0f;
|
|
quaternion->x = 0.0f;
|
|
quaternion->y = 0.0f;
|
|
quaternion->z = 0.0f;
|
|
return;
|
|
}
|
|
|
|
// unit_start and unit_end are opposite, angle = 180 degrees
|
|
quaternion->s = 0.0f;
|
|
quaternion->x = unit_orthogonal->x;
|
|
quaternion->y = unit_orthogonal->y;
|
|
quaternion->z = unit_orthogonal->z;
|
|
return;
|
|
}
|
|
|
|
const float axis_modulus = sqrtf(axis_square_modulus);
|
|
|
|
const float angle = 0.5f * atan2f(axis_modulus, dot_product);
|
|
|
|
const float multiplier = sinf(angle) / axis_modulus;
|
|
|
|
quaternion->s = cosf(angle);
|
|
quaternion->x = axis.x * multiplier;
|
|
quaternion->y = axis.y * multiplier;
|
|
quaternion->z = axis.z * multiplier;
|
|
}
|
|
|
|
static inline void _bgc_fp64_turn3_get_turning_quaternion(BGC_FP64_Quaternion* const quaternion, const BGC_FP64_Vector3* const unit_start, const BGC_FP64_Vector3* const unit_end, const BGC_FP64_Vector3* const unit_orthogonal)
|
|
{
|
|
BGC_FP64_Vector3 axis;
|
|
|
|
bgc_fp64_vector3_get_cross_product(&axis, unit_start, unit_end);
|
|
|
|
const double dot_product = bgc_fp64_vector3_get_dot_product(unit_start, unit_end);
|
|
|
|
const double axis_square_modulus = bgc_fp64_vector3_get_squared_length(&axis);
|
|
|
|
// unit_start and unit_end are parallel
|
|
if (axis_square_modulus <= BGC_FP64_SQUARE_EPSILON) {
|
|
// unit_start and unit_end are co-directional, angle = 180 degrees
|
|
if (dot_product >= 0.0) {
|
|
quaternion->s = 1.0;
|
|
quaternion->x = 0.0;
|
|
quaternion->y = 0.0;
|
|
quaternion->z = 0.0;
|
|
return;
|
|
}
|
|
|
|
// unit_start and unit_end are opposite, angle = 180 degrees
|
|
quaternion->s = 0.0;
|
|
quaternion->x = unit_orthogonal->x;
|
|
quaternion->y = unit_orthogonal->y;
|
|
quaternion->z = unit_orthogonal->z;
|
|
return;
|
|
}
|
|
|
|
const double axis_modulus = sqrt(axis_square_modulus);
|
|
|
|
const double angle = 0.5 * atan2(axis_modulus, dot_product);
|
|
|
|
const double multiplier = sin(angle) / axis_modulus;
|
|
|
|
quaternion->s = cos(angle);
|
|
quaternion->x = axis.x * multiplier;
|
|
quaternion->y = axis.y * multiplier;
|
|
quaternion->z = axis.z * multiplier;
|
|
}
|
|
|
|
// ============ Make Pair Difference ============ //
|
|
|
|
int bgc_fp32_turn3_find_pair_difference(
|
|
BGC_FP32_Turn3* const turn,
|
|
const BGC_FP32_Vector3* const first_pair_main,
|
|
const BGC_FP32_Vector3* const first_pair_branch,
|
|
const BGC_FP32_Vector3* const second_pair_main,
|
|
const BGC_FP32_Vector3* const second_pair_branch
|
|
) {
|
|
BGC_FP32_Vector3 first_fixed_main, first_fixed_branch, first_turned_branch, second_fixed_main, second_fixed_branch;
|
|
|
|
int status = _bgc_fp32_turn3_get_orthogonal_pair(&first_fixed_main, &first_fixed_branch, first_pair_main, first_pair_branch);
|
|
|
|
if (status != BGC_SUCCESS) {
|
|
bgc_fp32_turn3_reset(turn);
|
|
return status + _BGC_ERROR_TURN3_FIRST_PAIR;
|
|
}
|
|
|
|
status = _bgc_fp32_turn3_get_orthogonal_pair(&second_fixed_main, &second_fixed_branch, second_pair_main, second_pair_branch);
|
|
|
|
if (status != BGC_SUCCESS) {
|
|
bgc_fp32_turn3_reset(turn);
|
|
return status + _BGC_ERROR_TURN3_SECOND_PAIR;
|
|
}
|
|
|
|
BGC_FP32_Quaternion q1, q2;
|
|
|
|
// Calculation of a turn (q1) which turns first_fixed_main into second_fixed_main
|
|
_bgc_fp32_turn3_get_turning_quaternion(&q1, &first_fixed_main, &second_fixed_main, &first_fixed_branch);
|
|
|
|
// Roughly turn first_fixed_branch with q1 turn
|
|
_bgc_fp32_versor_turn_vector(&first_turned_branch, &q1, &first_fixed_branch);
|
|
|
|
// Calculation of a turn (q2) which turns first_turned_branch into second_fixed_branch
|
|
_bgc_fp32_turn3_get_turning_quaternion(&q2, &first_turned_branch, &second_fixed_branch, &second_fixed_main);
|
|
|
|
// Composing two turns with multiplication of quaterntions (q2 * q1)
|
|
bgc_fp32_quaternion_multiply_by_quaternion(&turn->_versor, &q2, &q1);
|
|
|
|
// Making a final versor (a normalized quaternion)
|
|
_bgc_fp32_turn3_normalize(turn);
|
|
|
|
return BGC_SUCCESS;
|
|
}
|
|
|
|
int bgc_fp64_turn3_find_pair_difference(
|
|
BGC_FP64_Turn3* const turn,
|
|
const BGC_FP64_Vector3* const first_pair_main,
|
|
const BGC_FP64_Vector3* const first_pair_branch,
|
|
const BGC_FP64_Vector3* const second_pair_main,
|
|
const BGC_FP64_Vector3* const second_pair_branch
|
|
) {
|
|
BGC_FP64_Vector3 first_fixed_main, first_fixed_branch, first_turned_branch, second_fixed_main, second_fixed_branch;
|
|
|
|
int status = _bgc_fp64_turn3_get_orthogonal_pair(&first_fixed_main, &first_fixed_branch, first_pair_main, first_pair_branch);
|
|
|
|
if (status != BGC_SUCCESS) {
|
|
bgc_fp64_turn3_reset(turn);
|
|
return status + _BGC_ERROR_TURN3_FIRST_PAIR;
|
|
}
|
|
|
|
status = _bgc_fp64_turn3_get_orthogonal_pair(&second_fixed_main, &second_fixed_branch, second_pair_main, second_pair_branch);
|
|
|
|
if (status != BGC_SUCCESS) {
|
|
bgc_fp64_turn3_reset(turn);
|
|
return status + _BGC_ERROR_TURN3_SECOND_PAIR;
|
|
}
|
|
|
|
BGC_FP64_Quaternion q1, q2;
|
|
|
|
// Calculation of a turn (q1) which turns first_fixed_main into second_fixed_main
|
|
_bgc_fp64_turn3_get_turning_quaternion(&q1, &first_fixed_main, &second_fixed_main, &first_fixed_branch);
|
|
|
|
// Roughly turn first_fixed_branch with q1 turn
|
|
_bgc_fp64_versor_turn_vector(&first_turned_branch, &q1, &first_fixed_branch);
|
|
|
|
// Calculation of a turn (q2) which turns first_turned_branch into second_fixed_branch
|
|
_bgc_fp64_turn3_get_turning_quaternion(&q2, &first_turned_branch, &second_fixed_branch, &second_fixed_main);
|
|
|
|
// Composing two turns with multiplication of quaterntions (q2 * q1)
|
|
bgc_fp64_quaternion_multiply_by_quaternion(&turn->_versor, &q2, &q1);
|
|
|
|
// Making a final versor (a normalized quaternion)
|
|
_bgc_fp64_turn3_normalize(turn);
|
|
|
|
return BGC_SUCCESS;
|
|
}
|
|
|
|
// =============== Get Exponation =============== //
|
|
|
|
void bgc_fp32_turn3_get_power(BGC_FP32_Turn3* const power, const BGC_FP32_Turn3* const base, const float exponent)
|
|
{
|
|
const float square_vector = base->_versor.x * base->_versor.x + base->_versor.y * base->_versor.y + base->_versor.z * base->_versor.z;
|
|
|
|
if (square_vector <= BGC_FP32_SQUARE_EPSILON || square_vector != square_vector) {
|
|
bgc_fp32_turn3_reset(power);
|
|
return;
|
|
}
|
|
|
|
const float vector_modulus = sqrtf(square_vector);
|
|
|
|
const float angle = atan2f(vector_modulus, base->_versor.s) * exponent;
|
|
|
|
const float multiplier = sinf(angle) / vector_modulus;
|
|
|
|
bgc_fp32_turn3_set_values(power, cosf(angle), base->_versor.x * multiplier, base->_versor.y * multiplier, base->_versor.z * multiplier);
|
|
}
|
|
|
|
void bgc_fp64_turn3_get_power(BGC_FP64_Turn3* const power, const BGC_FP64_Turn3* const base, const double exponent)
|
|
{
|
|
const double square_vector = base->_versor.x * base->_versor.x + base->_versor.y * base->_versor.y + base->_versor.z * base->_versor.z;
|
|
|
|
if (square_vector <= BGC_FP64_SQUARE_EPSILON || square_vector != square_vector) {
|
|
bgc_fp64_turn3_reset(power);
|
|
return;
|
|
}
|
|
|
|
const double vector_modulus = sqrt(square_vector);
|
|
|
|
const double angle = atan2(vector_modulus, base->_versor.s) * exponent;
|
|
|
|
const double multiplier = sin(angle) / vector_modulus;
|
|
|
|
bgc_fp64_turn3_set_values(power, cos(angle), base->_versor.x * multiplier, base->_versor.y * multiplier, base->_versor.z * multiplier);
|
|
}
|
|
|
|
// ============ Sphere Interpolation ============ //
|
|
|
|
void bgc_fp32_turn3_spherically_interpolate(BGC_FP32_Turn3* const interpolation, const BGC_FP32_Turn3* const start, const BGC_FP32_Turn3* const end, const float phase)
|
|
{
|
|
const float delta_s = (end->_versor.s * start->_versor.s + end->_versor.x * start->_versor.x) + (end->_versor.y * start->_versor.y + end->_versor.z * start->_versor.z);
|
|
const float delta_x = (end->_versor.x * start->_versor.s + end->_versor.z * start->_versor.y) - (end->_versor.s * start->_versor.x + end->_versor.y * start->_versor.z);
|
|
const float delta_y = (end->_versor.y * start->_versor.s + end->_versor.x * start->_versor.z) - (end->_versor.s * start->_versor.y + end->_versor.z * start->_versor.x);
|
|
const float delta_z = (end->_versor.z * start->_versor.s + end->_versor.y * start->_versor.x) - (end->_versor.s * start->_versor.z + end->_versor.x * start->_versor.y);
|
|
|
|
const float square_vector = delta_x * delta_x + delta_y * delta_y + delta_z * delta_z;
|
|
|
|
// square_vector != square_vector means checking for NaN value at square_vector
|
|
if (square_vector <= BGC_FP32_SQUARE_EPSILON || isnan(square_vector)) {
|
|
bgc_fp32_turn3_copy(interpolation, end);
|
|
return;
|
|
}
|
|
|
|
// Calculating of the turning which fits the phase:
|
|
const float vector_modulus = sqrtf(square_vector);
|
|
const float angle = atan2f(vector_modulus, delta_s) * phase;
|
|
const float multiplier = sinf(angle) / vector_modulus;
|
|
|
|
const float turn_s = cosf(angle);
|
|
const float turn_x = delta_x * multiplier;
|
|
const float turn_y = delta_y * multiplier;
|
|
const float turn_z = delta_z * multiplier;
|
|
|
|
// Combining of starting orientation with the turning
|
|
bgc_fp32_turn3_set_values(
|
|
interpolation,
|
|
(turn_s * start->_versor.s - turn_x * start->_versor.x) - (turn_y * start->_versor.y + turn_z * start->_versor.z),
|
|
(turn_x * start->_versor.s + turn_s * start->_versor.x) - (turn_z * start->_versor.y - turn_y * start->_versor.z),
|
|
(turn_y * start->_versor.s + turn_s * start->_versor.y) - (turn_x * start->_versor.z - turn_z * start->_versor.x),
|
|
(turn_z * start->_versor.s + turn_s * start->_versor.z) - (turn_y * start->_versor.x - turn_x * start->_versor.y)
|
|
);
|
|
}
|
|
|
|
void bgc_fp64_turn3_spherically_interpolate(BGC_FP64_Turn3* const interpolation, const BGC_FP64_Turn3* const start, const BGC_FP64_Turn3* const end, const double phase)
|
|
{
|
|
const double delta_s = (end->_versor.s * start->_versor.s + end->_versor.x * start->_versor.x) + (end->_versor.y * start->_versor.y + end->_versor.z * start->_versor.z);
|
|
const double delta_x = (end->_versor.x * start->_versor.s + end->_versor.z * start->_versor.y) - (end->_versor.s * start->_versor.x + end->_versor.y * start->_versor.z);
|
|
const double delta_y = (end->_versor.y * start->_versor.s + end->_versor.x * start->_versor.z) - (end->_versor.s * start->_versor.y + end->_versor.z * start->_versor.x);
|
|
const double delta_z = (end->_versor.z * start->_versor.s + end->_versor.y * start->_versor.x) - (end->_versor.s * start->_versor.z + end->_versor.x * start->_versor.y);
|
|
|
|
const double square_vector = delta_x * delta_x + delta_y * delta_y + delta_z * delta_z;
|
|
|
|
// square_vector != square_vector means checking for NaN value at square_vector
|
|
if (square_vector <= BGC_FP64_SQUARE_EPSILON || isnan(square_vector)) {
|
|
bgc_fp64_turn3_copy(interpolation, end);
|
|
return;
|
|
}
|
|
|
|
// Calculating of the turning which fits the phase:
|
|
const double vector_modulus = sqrt(square_vector);
|
|
const double angle = atan2(vector_modulus, delta_s) * phase;
|
|
const double multiplier = sin(angle) / vector_modulus;
|
|
|
|
const double turn_s = cos(angle);
|
|
const double turn_x = delta_x * multiplier;
|
|
const double turn_y = delta_y * multiplier;
|
|
const double turn_z = delta_z * multiplier;
|
|
|
|
// Combining of starting orientation with the turning
|
|
bgc_fp64_turn3_set_values(
|
|
interpolation,
|
|
(turn_s * start->_versor.s - turn_x * start->_versor.x) - (turn_y * start->_versor.y + turn_z * start->_versor.z),
|
|
(turn_x * start->_versor.s + turn_s * start->_versor.x) - (turn_z * start->_versor.y - turn_y * start->_versor.z),
|
|
(turn_y * start->_versor.s + turn_s * start->_versor.y) - (turn_x * start->_versor.z - turn_z * start->_versor.x),
|
|
(turn_z * start->_versor.s + turn_s * start->_versor.z) - (turn_y * start->_versor.x - turn_x * start->_versor.y)
|
|
);
|
|
}
|