593 lines
27 KiB
C
593 lines
27 KiB
C
#include <math.h>
|
|
|
|
#include "angle.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 }};
|
|
|
|
extern inline void bgc_fp32_turn3_reset(BGC_FP32_Turn3* turn);
|
|
extern inline void bgc_fp64_turn3_reset(BGC_FP64_Turn3* turn);
|
|
|
|
extern inline void bgc_fp32_turn3_set_raw_values(BGC_FP32_Turn3* 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* turn, const double s0, const double x1, const double x2, const double x3);
|
|
|
|
extern inline void bgc_fp32_turn3_get_quaternion(BGC_FP32_Quaternion* quaternion, const BGC_FP32_Turn3* turn);
|
|
extern inline void bgc_fp64_turn3_get_quaternion(BGC_FP64_Quaternion* quaternion, const BGC_FP64_Turn3* turn);
|
|
|
|
extern inline void bgc_fp32_turn3_set_quaternion(BGC_FP32_Turn3* turn, const BGC_FP32_Quaternion* quaternion);
|
|
extern inline void bgc_fp64_turn3_set_quaternion(BGC_FP64_Turn3* turn, const BGC_FP64_Quaternion* quaternion);
|
|
|
|
extern inline void bgc_fp32_turn3_copy(BGC_FP32_Turn3* destination, const BGC_FP32_Turn3* source);
|
|
extern inline void bgc_fp64_turn3_copy(BGC_FP64_Turn3* destination, const BGC_FP64_Turn3* source);
|
|
|
|
extern inline void bgc_fp32_turn3_swap(BGC_FP32_Turn3* turn1, BGC_FP32_Turn3* turn2);
|
|
extern inline void bgc_fp64_turn3_swap(BGC_FP64_Turn3* turn1, BGC_FP64_Turn3* turn2);
|
|
|
|
extern inline int bgc_fp32_turn3_is_idle(const BGC_FP32_Turn3* turn);
|
|
extern inline int bgc_fp64_turn3_is_idle(const BGC_FP64_Turn3* turn);
|
|
|
|
extern inline void bgc_fp32_turn3_convert_to_fp64(BGC_FP64_Turn3* destination, const BGC_FP32_Turn3* source);
|
|
extern inline void bgc_fp64_turn3_convert_to_fp32(BGC_FP32_Turn3* destination, const BGC_FP64_Turn3* source);
|
|
|
|
extern inline void bgc_fp32_turn3_shorten(BGC_FP32_Turn3* turn);
|
|
extern inline void bgc_fp64_turn3_shorten(BGC_FP64_Turn3* turn);
|
|
|
|
extern inline void bgc_fp32_turn3_get_shortened(BGC_FP32_Turn3* shortened, const BGC_FP32_Turn3* turn);
|
|
extern inline void bgc_fp64_turn3_get_shortened(BGC_FP64_Turn3* shortened, const BGC_FP64_Turn3* turn);
|
|
|
|
extern inline void bgc_fp32_turn3_alternate(BGC_FP32_Turn3* turn);
|
|
extern inline void bgc_fp64_turn3_alternate(BGC_FP64_Turn3* turn);
|
|
|
|
extern inline void bgc_fp32_turn3_get_alternative(BGC_FP32_Turn3* alternative, const BGC_FP32_Turn3* turn);
|
|
extern inline void bgc_fp64_turn3_get_alternative(BGC_FP64_Turn3* alternative, const BGC_FP64_Turn3* turn);
|
|
|
|
extern inline void bgc_fp32_turn3_revert(BGC_FP32_Turn3* turn);
|
|
extern inline void bgc_fp64_turn3_revert(BGC_FP64_Turn3* turn);
|
|
|
|
extern inline void bgc_fp32_turn3_get_reverse(BGC_FP32_Turn3* inverse, const BGC_FP32_Turn3* turn);
|
|
extern inline void bgc_fp64_turn3_get_reverse(BGC_FP64_Turn3* inverse, const BGC_FP64_Turn3* turn);
|
|
|
|
extern inline void bgc_fp32_turn3_combine(BGC_FP32_Turn3* combination, const BGC_FP32_Turn3* first, const BGC_FP32_Turn3* second);
|
|
extern inline void bgc_fp64_turn3_combine(BGC_FP64_Turn3* combination, const BGC_FP64_Turn3* first, const BGC_FP64_Turn3* second);
|
|
|
|
extern inline void bgc_fp32_turn3_combine3(BGC_FP32_Turn3* combination, const BGC_FP32_Turn3* first, const BGC_FP32_Turn3* second, const BGC_FP32_Turn3* third);
|
|
extern inline void bgc_fp64_turn3_combine3(BGC_FP64_Turn3* combination, const BGC_FP64_Turn3* first, const BGC_FP64_Turn3* second, const BGC_FP64_Turn3* third);
|
|
|
|
extern inline void bgc_fp32_turn3_exclude(BGC_FP32_Turn3* difference, const BGC_FP32_Turn3* base, const BGC_FP32_Turn3* excludant);
|
|
extern inline void bgc_fp64_turn3_exclude(BGC_FP64_Turn3* difference, const BGC_FP64_Turn3* base, const BGC_FP64_Turn3* excludant);
|
|
|
|
extern inline void bgc_fp32_turn3_get_rotation_matrix(BGC_FP32_Matrix3x3* matrix, const BGC_FP32_Turn3* turn);
|
|
extern inline void bgc_fp64_turn3_get_rotation_matrix(BGC_FP64_Matrix3x3* matrix, const BGC_FP64_Turn3* turn);
|
|
|
|
extern inline void bgc_fp32_turn3_get_reverse_matrix(BGC_FP32_Matrix3x3* matrix, const BGC_FP32_Turn3* turn);
|
|
extern inline void bgc_fp64_turn3_get_reverse_matrix(BGC_FP64_Matrix3x3* matrix, const BGC_FP64_Turn3* turn);
|
|
|
|
extern inline void bgc_fp32_turn3_get_both_matrices(BGC_FP32_Matrix3x3* rotation, BGC_FP32_Matrix3x3* reverse, const BGC_FP32_Turn3* turn);
|
|
extern inline void bgc_fp64_turn3_get_both_matrices(BGC_FP64_Matrix3x3* rotation, BGC_FP64_Matrix3x3* reverse, const BGC_FP64_Turn3* turn);
|
|
|
|
extern inline void bgc_fp32_turn3_vector(BGC_FP32_Vector3* turned_vector, const BGC_FP32_Turn3* turn, const BGC_FP32_Vector3* vector);
|
|
extern inline void bgc_fp64_turn3_vector(BGC_FP64_Vector3* turned_vector, const BGC_FP64_Turn3* turn, const BGC_FP64_Vector3* vector);
|
|
|
|
extern inline void bgc_fp32_turn3_vector_back(BGC_FP32_Vector3* turned_vector, const BGC_FP32_Turn3* turn, const BGC_FP32_Vector3* vector);
|
|
extern inline void bgc_fp64_turn3_vector_back(BGC_FP64_Vector3* turned_vector, const BGC_FP64_Turn3* turn, const BGC_FP64_Vector3* vector);
|
|
|
|
extern inline int bgc_fp32_turn3_are_close(const BGC_FP32_Turn3* turn1, const BGC_FP32_Turn3* turn2);
|
|
extern inline int bgc_fp64_turn3_are_close(const BGC_FP64_Turn3* turn1, const BGC_FP64_Turn3* turn2);
|
|
|
|
// ================= Normalize ================== //
|
|
|
|
void _bgc_fp32_turn3_normalize(BGC_FP32_Turn3* 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(&turn->_versor, &turn->_versor, sqrtf(1.0f / square_modulus));
|
|
}
|
|
|
|
void _bgc_fp64_turn3_normalize(BGC_FP64_Turn3* 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(&turn->_versor, &turn->_versor, sqrt(1.0 / square_modulus));
|
|
}
|
|
|
|
|
|
// ================ Get Rotation ================ //
|
|
|
|
float bgc_fp32_turn3_get_rotation(BGC_FP32_Vector3* axis, const BGC_FP32_Turn3* turn, const int angle_unit)
|
|
{
|
|
const float square_vector_modulus = turn->_versor.x1 * turn->_versor.x1 + turn->_versor.x2 * turn->_versor.x2 + turn->_versor.x3 * turn->_versor.x3;
|
|
|
|
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->x1 = turn->_versor.x1 * multiplier;
|
|
axis->x2 = turn->_versor.x2 * multiplier;
|
|
axis->x3 = turn->_versor.x3 * multiplier;
|
|
|
|
return 2.0f * atan2f(vector_modulus, turn->_versor.s0);
|
|
}
|
|
|
|
double bgc_fp64_turn3_get_rotation(BGC_FP64_Vector3* axis, const BGC_FP64_Turn3* turn, const int angle_unit)
|
|
{
|
|
const double square_vector_modulus = turn->_versor.x1 * turn->_versor.x1 + turn->_versor.x2 * turn->_versor.x2 + turn->_versor.x3 * turn->_versor.x3;
|
|
|
|
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->x1 = turn->_versor.x1 * multiplier;
|
|
axis->x2 = turn->_versor.x2 * multiplier;
|
|
axis->x3 = turn->_versor.x3 * multiplier;
|
|
|
|
return 2.0 * atan2(vector_modulus, turn->_versor.s0);
|
|
}
|
|
|
|
// ================ Set Rotation ================ //
|
|
|
|
void bgc_fp32_turn3_set_rotation(BGC_FP32_Turn3* turn, const float x1, const float x2, const float x3, const float angle, const int angle_unit)
|
|
{
|
|
const float square_vector = x1 * x1 + x2 * x2 + x3 * x3;
|
|
|
|
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);
|
|
|
|
bgc_fp32_quaternion_make(&turn->_versor, cosf(half_angle), x1 * multiplier, x2 * multiplier, x3 * multiplier);
|
|
|
|
const float square_modulus = bgc_fp32_quaternion_get_square_modulus(&turn->_versor);
|
|
|
|
if (!bgc_fp32_is_square_unit(square_modulus)) {
|
|
_bgc_fp32_turn3_normalize(turn, square_modulus);
|
|
}
|
|
}
|
|
|
|
void bgc_fp64_turn3_set_rotation(BGC_FP64_Turn3* turn, const double x1, const double x2, const double x3, const double angle, const int angle_unit)
|
|
{
|
|
const double square_vector = x1 * x1 + x2 * x2 + x3 * x3;
|
|
|
|
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);
|
|
|
|
bgc_fp64_quaternion_make(&turn->_versor, cosf(half_angle), x1 * multiplier, x2 * multiplier, x3 * multiplier);
|
|
|
|
const double square_modulus = bgc_fp64_quaternion_get_square_modulus(&turn->_versor);
|
|
|
|
if (!bgc_fp64_is_square_unit(square_modulus)) {
|
|
_bgc_fp64_turn3_normalize(turn, square_modulus);
|
|
}
|
|
}
|
|
|
|
// ========= Make Direction Difference ========== //
|
|
|
|
static int _bgc_fp32_turn3_make_direction_turn(BGC_FP32_Turn3* turn, const BGC_FP32_Vector3* start, const BGC_FP32_Vector3* end, const float square_modulus)
|
|
{
|
|
BGC_FP32_Vector3 orthogonal_axis;
|
|
|
|
bgc_fp32_vector3_get_cross_product(&orthogonal_axis, start, end);
|
|
|
|
const float scalar_product = bgc_fp32_vector3_get_dot_product(start, end);
|
|
const float square_modulus = bgc_fp32_vector3_get_square_modulus(&orthogonal_axis);
|
|
const float square_sine = square_modulus / square_modulus_product;
|
|
|
|
if (square_sine > BGC_FP64_SQUARE_EPSILON) {
|
|
const float cosine = scalar_product / sqrtf(square_modulus_product);
|
|
const float angle = 0.5 * atan2f(sqrtf(square_sine), cosine);
|
|
|
|
const float multiplier = sin(angle) * sqrtf(1.0f / square_modulus);
|
|
|
|
bgc_fp32_turn3_set_raw_values(versor, cosf(angle), orthogonal_axis.x1 * multiplier, orthogonal_axis.x2 * multiplier, orthogonal_axis.x3 * multiplier);
|
|
return BGC_SOME_TURN;
|
|
}
|
|
|
|
if (scalar_product < 0.0f) {
|
|
return BGC_OPPOSITE;
|
|
}
|
|
|
|
bgc_fp32_turn3_reset(versor);
|
|
|
|
return BGC_ZERO_TURN;
|
|
}
|
|
|
|
static int _bgc_fp64_turn3_make_direction_turn(BGC_FP64_Turn3* versor, const BGC_FP64_Vector3* start, const BGC_FP64_Vector3* end, const double square_modulus_product)
|
|
{
|
|
BGC_FP64_Vector3 orthogonal_axis;
|
|
|
|
bgc_fp64_vector3_get_cross_product(&orthogonal_axis, start, end);
|
|
|
|
const double scalar_product = bgc_fp64_vector3_get_dot_product(start, end);
|
|
const double square_modulus = bgc_fp64_vector3_get_square_modulus(&orthogonal_axis);
|
|
const double square_sine = square_modulus / square_modulus_product;
|
|
|
|
if (square_sine > BGC_FP64_SQUARE_EPSILON) {
|
|
const double cosine = scalar_product / sqrt(square_modulus_product);
|
|
const double angle = 0.5 * atan2(sqrt(square_sine), cosine);
|
|
|
|
const double multiplier = sin(angle) * sqrt(1.0f / square_modulus);
|
|
|
|
bgc_fp64_turn3_set_raw_values(versor, cos(angle), orthogonal_axis.x1 * multiplier, orthogonal_axis.x2 * multiplier, orthogonal_axis.x3 * multiplier);
|
|
return BGC_SOME_TURN;
|
|
}
|
|
|
|
if (scalar_product < 0.0) {
|
|
return BGC_OPPOSITE;
|
|
}
|
|
|
|
bgc_fp64_turn3_reset(versor);
|
|
|
|
return BGC_ZERO_TURN;
|
|
}
|
|
|
|
// ========= Find Direction Difference ========== //
|
|
|
|
int bgc_fp32_turn3_find_direction_difference(BGC_FP32_Turn3* difference, const BGC_FP32_Vector3* start, const BGC_FP32_Vector3* end)
|
|
{
|
|
const float start_square_modulus = bgc_fp32_vector3_get_square_modulus(start);
|
|
const float end_square_modulus = bgc_fp32_vector3_get_square_modulus(end);
|
|
|
|
if (start_square_modulus <= BGC_FP32_SQUARE_EPSILON || end_square_modulus <= BGC_FP32_SQUARE_EPSILON) {
|
|
bgc_fp32_turn3_reset(difference);
|
|
return BGC_ZERO_TURN;
|
|
}
|
|
|
|
const double scalar_product = bgc_fp64_vector3_get_dot_product(start, end);
|
|
|
|
if () {
|
|
}
|
|
|
|
BGC_FP64_Vector3 orthogonal_axis;
|
|
|
|
bgc_fp64_vector3_get_cross_product(&orthogonal_axis, start, end);
|
|
|
|
return _bgc_fp32_turn3_make_direction_turn(difference, start, end, start_square_modulus * end_square_modulus);
|
|
}
|
|
|
|
int bgc_fp64_turn3_find_direction_difference(BGC_FP64_Turn3* difference, const BGC_FP64_Vector3* start, const BGC_FP64_Vector3* end)
|
|
{
|
|
const double start_square_modulus = bgc_fp64_vector3_get_square_modulus(start);
|
|
const double end_square_modulus = bgc_fp64_vector3_get_square_modulus(end);
|
|
|
|
if (start_square_modulus <= BGC_FP64_SQUARE_EPSILON || end_square_modulus <= BGC_FP64_SQUARE_EPSILON) {
|
|
bgc_fp64_turn3_reset(difference);
|
|
return BGC_ZERO_TURN;
|
|
}
|
|
|
|
return _bgc_fp64_turn3_make_direction_turn(difference, start, end, start_square_modulus * end_square_modulus);
|
|
}
|
|
|
|
// =============== Set Directions =============== //
|
|
|
|
static int _bgc_fp32_turn3_validate_basis(const float primary_square_modulus, const float auxiliary_square_modulus, const float orthogonal_square_modulus)
|
|
{
|
|
if (primary_square_modulus <= BGC_FP32_SQUARE_EPSILON) {
|
|
//TODO: add error code for: primary_vector is zero
|
|
return BGC_FAILED;
|
|
}
|
|
|
|
if (auxiliary_square_modulus <= BGC_FP32_SQUARE_EPSILON) {
|
|
//TODO: add error code for: auxiliary_vector is zero
|
|
return BGC_FAILED;
|
|
}
|
|
|
|
if (orthogonal_square_modulus <= BGC_FP32_SQUARE_EPSILON * primary_square_modulus * auxiliary_square_modulus) {
|
|
//TODO: add error code for: primary_vector and auxiliary_vector are parallel
|
|
return BGC_FAILED;
|
|
}
|
|
|
|
return BGC_SUCCESS;
|
|
}
|
|
|
|
static int _bgc_fp64_turn3_validate_basis(const double primary_square_modulus, const double auxiliary_square_modulus, const double orthogonal_square_modulus)
|
|
{
|
|
if (primary_square_modulus <= BGC_FP64_SQUARE_EPSILON) {
|
|
//TODO: add error code for: primary_vector is zero
|
|
return BGC_FAILED;
|
|
}
|
|
|
|
if (auxiliary_square_modulus <= BGC_FP64_SQUARE_EPSILON) {
|
|
//TODO: add error code for: auxiliary_vector is zero
|
|
return BGC_FAILED;
|
|
}
|
|
|
|
if (orthogonal_square_modulus <= BGC_FP64_SQUARE_EPSILON * primary_square_modulus * auxiliary_square_modulus) {
|
|
//TODO: add error code for: primary_vector and auxiliary_vector are parallel
|
|
return BGC_FAILED;
|
|
}
|
|
|
|
return BGC_SUCCESS;
|
|
}
|
|
|
|
int bgc_fp32_turn3_make_basis_difference(
|
|
BGC_FP32_Turn3* versor,
|
|
const BGC_FP32_Vector3* initial_primary_direction,
|
|
const BGC_FP32_Vector3* initial_auxiliary_direction,
|
|
const BGC_FP32_Vector3* final_primary_direction,
|
|
const BGC_FP32_Vector3* final_auxiliary_direction
|
|
)
|
|
{
|
|
BGC_FP32_Vector3 initial_orthogonal_direction, turned_orthogonal_direction, final_orthogonal_direction;
|
|
|
|
// Step 1: Validate initial basis:
|
|
bgc_fp32_vector3_get_cross_product(&initial_orthogonal_direction, initial_primary_direction, initial_auxiliary_direction);
|
|
|
|
const float initial_primary_square_modulus = bgc_fp32_vector3_get_square_modulus(initial_primary_direction);
|
|
const float initial_auxiliary_square_modulus = bgc_fp32_vector3_get_square_modulus(initial_auxiliary_direction);
|
|
const float initial_orthogonal_square_modulus = bgc_fp32_vector3_get_square_modulus(&initial_orthogonal_direction);
|
|
|
|
const int initial_basis_valudation = _bgc_fp32_turn3_validate_basis(initial_primary_square_modulus, initial_auxiliary_square_modulus, initial_orthogonal_square_modulus);
|
|
|
|
if (initial_basis_valudation != BGC_SUCCESS) {
|
|
return initial_basis_valudation;
|
|
}
|
|
|
|
// Step 1: Validate final basis:
|
|
bgc_fp32_vector3_get_cross_product(&final_orthogonal_direction, final_primary_direction, final_auxiliary_direction);
|
|
|
|
const float final_primary_square_modulus = bgc_fp32_vector3_get_square_modulus(final_primary_direction);
|
|
const float final_auxiliary_square_modulus = bgc_fp32_vector3_get_square_modulus(final_auxiliary_direction);
|
|
const float final_orthogonal_square_modulus = bgc_fp32_vector3_get_square_modulus(&final_orthogonal_direction);
|
|
|
|
const int final_basis_valudation = _bgc_fp32_turn3_validate_basis(final_primary_square_modulus, final_auxiliary_square_modulus, final_orthogonal_square_modulus);
|
|
|
|
if (final_basis_valudation != BGC_SUCCESS) {
|
|
return final_basis_valudation;
|
|
}
|
|
|
|
// Step 3: Validate normalize orthogonal vectors:
|
|
bgc_fp32_vector3_divide(&initial_orthogonal_direction, &initial_orthogonal_direction, sqrtf(initial_orthogonal_square_modulus));
|
|
bgc_fp32_vector3_divide(&final_orthogonal_direction, &final_orthogonal_direction, sqrtf(final_orthogonal_square_modulus));
|
|
|
|
BGC_FP32_Turn3 turn1, turn2;
|
|
|
|
// Step 4: Find turn1
|
|
int turn1_code = _bgc_fp32_turn3_make_direction_turn(&turn1, initial_primary_direction, final_primary_direction, initial_primary_square_modulus * final_primary_square_modulus);
|
|
|
|
if (turn1_code == BGC_OPPOSITE) {
|
|
bgc_fp32_turn3_set_raw_values(&turn1, 0.0f, initial_orthogonal_direction.x1, initial_orthogonal_direction.x2, initial_orthogonal_direction.x3);
|
|
}
|
|
|
|
bgc_fp32_turn3_vector(&turned_orthogonal_direction, &turn1, &initial_orthogonal_direction);
|
|
|
|
// Step 5: Find turn2:
|
|
int turn2_code = _bgc_fp32_turn3_make_direction_turn(&turn2, &turned_orthogonal_direction, &final_orthogonal_direction, 1.0f);
|
|
|
|
if (turn2_code == BGC_OPPOSITE) {
|
|
const float turn2_multiplier = sqrtf(1.0f / final_primary_square_modulus);
|
|
|
|
bgc_fp32_turn3_set_raw_values(&turn2,
|
|
0.0f,
|
|
final_primary_direction->x1 * turn2_multiplier,
|
|
final_primary_direction->x2 * turn2_multiplier,
|
|
final_primary_direction->x3 * turn2_multiplier
|
|
);
|
|
}
|
|
|
|
// Step 6: Combine turn1 and turn2:
|
|
bgc_fp32_turn3_combine(versor, &turn1, &turn2);
|
|
|
|
return BGC_SUCCESS;
|
|
}
|
|
|
|
int bgc_fp64_turn3_make_basis_difference(
|
|
BGC_FP64_Turn3* versor,
|
|
const BGC_FP64_Vector3* initial_primary_direction,
|
|
const BGC_FP64_Vector3* initial_auxiliary_direction,
|
|
const BGC_FP64_Vector3* final_primary_direction,
|
|
const BGC_FP64_Vector3* final_auxiliary_direction
|
|
)
|
|
{
|
|
BGC_FP64_Vector3 initial_orthogonal_direction, turned_orthogonal_direction, final_orthogonal_direction;
|
|
|
|
// Step 1: Validate initial basis:
|
|
bgc_fp64_vector3_get_cross_product(&initial_orthogonal_direction, initial_primary_direction, initial_auxiliary_direction);
|
|
|
|
const double initial_primary_square_modulus = bgc_fp64_vector3_get_square_modulus(initial_primary_direction);
|
|
const double initial_auxiliary_square_modulus = bgc_fp64_vector3_get_square_modulus(initial_auxiliary_direction);
|
|
const double initial_orthogonal_square_modulus = bgc_fp64_vector3_get_square_modulus(&initial_orthogonal_direction);
|
|
|
|
const int initial_basis_valudation = _bgc_fp64_turn3_validate_basis(initial_primary_square_modulus, initial_auxiliary_square_modulus, initial_orthogonal_square_modulus);
|
|
|
|
if (initial_basis_valudation != BGC_SUCCESS) {
|
|
return initial_basis_valudation;
|
|
}
|
|
|
|
// Step 1: Validate final basis:
|
|
bgc_fp64_vector3_get_cross_product(&final_orthogonal_direction, final_primary_direction, final_auxiliary_direction);
|
|
|
|
const double final_primary_square_modulus = bgc_fp64_vector3_get_square_modulus(final_primary_direction);
|
|
const double final_auxiliary_square_modulus = bgc_fp64_vector3_get_square_modulus(final_auxiliary_direction);
|
|
const double final_orthogonal_square_modulus = bgc_fp64_vector3_get_square_modulus(&final_orthogonal_direction);
|
|
|
|
const int final_basis_valudation = _bgc_fp64_turn3_validate_basis(final_primary_square_modulus, final_auxiliary_square_modulus, final_orthogonal_square_modulus);
|
|
|
|
if (final_basis_valudation != BGC_SUCCESS) {
|
|
return final_basis_valudation;
|
|
}
|
|
|
|
// Step 3: Validate normalize orthogonal vectors:
|
|
bgc_fp64_vector3_divide(&initial_orthogonal_direction, &initial_orthogonal_direction, sqrt(initial_orthogonal_square_modulus));
|
|
bgc_fp64_vector3_divide(&final_orthogonal_direction, &final_orthogonal_direction, sqrt(final_orthogonal_square_modulus));
|
|
|
|
BGC_FP64_Turn3 turn1, turn2;
|
|
|
|
// Step 4: Find turn1
|
|
int turn1_code = _bgc_fp64_turn3_make_direction_turn(&turn1, initial_primary_direction, final_primary_direction, initial_primary_square_modulus * final_primary_square_modulus);
|
|
|
|
if (turn1_code == BGC_OPPOSITE) {
|
|
bgc_fp64_turn3_set_raw_values(&turn1, 0.0, initial_orthogonal_direction.x1, initial_orthogonal_direction.x2, initial_orthogonal_direction.x3);
|
|
}
|
|
|
|
bgc_fp64_turn3_vector(&turned_orthogonal_direction, &turn1, &initial_orthogonal_direction);
|
|
|
|
// Step 5: Find turn2:
|
|
int turn2_code = _bgc_fp64_turn3_make_direction_turn(&turn2, &turned_orthogonal_direction, &final_orthogonal_direction, 1.0f);
|
|
|
|
if (turn2_code == BGC_OPPOSITE) {
|
|
const double turn2_multiplier = sqrt(1.0 / final_primary_square_modulus);
|
|
|
|
bgc_fp64_turn3_set_raw_values(&turn2,
|
|
0.0,
|
|
final_primary_direction->x1 * turn2_multiplier,
|
|
final_primary_direction->x2 * turn2_multiplier,
|
|
final_primary_direction->x3 * turn2_multiplier
|
|
);
|
|
}
|
|
|
|
// Step 6: Combine turn1 and turn2:
|
|
bgc_fp64_turn3_combine(versor, &turn1, &turn2);
|
|
|
|
return BGC_SUCCESS;
|
|
}
|
|
|
|
// =============== Get Exponation =============== //
|
|
|
|
void bgc_fp32_turn3_get_exponation(BGC_FP32_Turn3* power, const BGC_FP32_Turn3* base, const float exponent)
|
|
{
|
|
const float square_vector = base->_versor.x1 * base->_versor.x1 + base->_versor.x2 * base->_versor.x2 + base->_versor.x3 * base->_versor.x3;
|
|
|
|
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.s0) * exponent;
|
|
|
|
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);
|
|
}
|
|
|
|
void bgc_fp64_turn3_get_exponation(BGC_FP64_Turn3* power, const BGC_FP64_Turn3* base, const double exponent)
|
|
{
|
|
const double square_vector = base->_versor.x1 * base->_versor.x1 + base->_versor.x2 * base->_versor.x2 + base->_versor.x3 * base->_versor.x3;
|
|
|
|
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.s0) * exponent;
|
|
|
|
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);
|
|
}
|
|
|
|
// ============ Sphere Interpolation ============ //
|
|
|
|
void bgc_fp32_turn3_spherically_interpolate(BGC_FP32_Turn3* interpolation, const BGC_FP32_Turn3* start, const BGC_FP32_Turn3* end, const float phase)
|
|
{
|
|
const float delta_s0 = (end->_versor.s0 * start->_versor.s0 + end->_versor.x1 * start->_versor.x1) + (end->_versor.x2 * start->_versor.x2 + end->_versor.x3 * start->_versor.x3);
|
|
const float delta_x1 = (end->_versor.x1 * start->_versor.s0 + end->_versor.x3 * start->_versor.x2) - (end->_versor.s0 * start->_versor.x1 + end->_versor.x2 * start->_versor.x3);
|
|
const float delta_x2 = (end->_versor.x2 * start->_versor.s0 + end->_versor.x1 * start->_versor.x3) - (end->_versor.s0 * start->_versor.x2 + end->_versor.x3 * start->_versor.x1);
|
|
const float delta_x3 = (end->_versor.x3 * start->_versor.s0 + end->_versor.x2 * start->_versor.x1) - (end->_versor.s0 * start->_versor.x3 + end->_versor.x1 * start->_versor.x2);
|
|
|
|
const float square_vector = delta_x1 * delta_x1 + delta_x2 * delta_x2 + delta_x3 * delta_x3;
|
|
|
|
// 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_s0) * phase;
|
|
const float multiplier = sinf(angle) / vector_modulus;
|
|
|
|
const float turn_s0 = cosf(angle);
|
|
const float turn_x1 = delta_x1 * multiplier;
|
|
const float turn_x2 = delta_x2 * multiplier;
|
|
const float turn_x3 = delta_x3 * multiplier;
|
|
|
|
// Combining of starting orientation with the turning
|
|
bgc_fp32_turn3_set_raw_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),
|
|
(turn_x2 * start->_versor.s0 + turn_s0 * start->_versor.x2) - (turn_x1 * start->_versor.x3 - turn_x3 * start->_versor.x1),
|
|
(turn_x3 * start->_versor.s0 + turn_s0 * start->_versor.x3) - (turn_x2 * start->_versor.x1 - turn_x1 * start->_versor.x2)
|
|
);
|
|
}
|
|
|
|
void bgc_fp64_turn3_spherically_interpolate(BGC_FP64_Turn3* interpolation, const BGC_FP64_Turn3* start, const BGC_FP64_Turn3* end, const double phase)
|
|
{
|
|
const double delta_s0 = (end->_versor.s0 * start->_versor.s0 + end->_versor.x1 * start->_versor.x1) + (end->_versor.x2 * start->_versor.x2 + end->_versor.x3 * start->_versor.x3);
|
|
const double delta_x1 = (end->_versor.x1 * start->_versor.s0 + end->_versor.x3 * start->_versor.x2) - (end->_versor.s0 * start->_versor.x1 + end->_versor.x2 * start->_versor.x3);
|
|
const double delta_x2 = (end->_versor.x2 * start->_versor.s0 + end->_versor.x1 * start->_versor.x3) - (end->_versor.s0 * start->_versor.x2 + end->_versor.x3 * start->_versor.x1);
|
|
const double delta_x3 = (end->_versor.x3 * start->_versor.s0 + end->_versor.x2 * start->_versor.x1) - (end->_versor.s0 * start->_versor.x3 + end->_versor.x1 * start->_versor.x2);
|
|
|
|
const double square_vector = delta_x1 * delta_x1 + delta_x2 * delta_x2 + delta_x3 * delta_x3;
|
|
|
|
// 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_s0) * phase;
|
|
const double multiplier = sin(angle) / vector_modulus;
|
|
|
|
const double turn_s0 = cos(angle);
|
|
const double turn_x1 = delta_x1 * multiplier;
|
|
const double turn_x2 = delta_x2 * multiplier;
|
|
const double turn_x3 = delta_x3 * multiplier;
|
|
|
|
// Combining of starting orientation with the turning
|
|
bgc_fp64_turn3_set_raw_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),
|
|
(turn_x2 * start->_versor.s0 + turn_s0 * start->_versor.x2) - (turn_x1 * start->_versor.x3 - turn_x3 * start->_versor.x1),
|
|
(turn_x3 * start->_versor.s0 + turn_s0 * start->_versor.x3) - (turn_x2 * start->_versor.x1 - turn_x1 * start->_versor.x2)
|
|
);
|
|
}
|