Исправление функции, которая находит трёхмерных поворот между двумя парами векторов
This commit is contained in:
parent
57280ac3f3
commit
2ce4b64ca3
7 changed files with 705 additions and 467 deletions
|
|
@ -85,7 +85,7 @@ void _bgc_fp32_turn3_normalize(BGC_FP32_Turn3* turn, const float square_modulus)
|
|||
return;
|
||||
}
|
||||
|
||||
bgc_fp32_quaternion_multiply(&turn->_versor, &turn->_versor, sqrtf(1.0f / square_modulus));
|
||||
bgc_fp32_quaternion_multiply_by_number(&turn->_versor, &turn->_versor, sqrtf(1.0f / square_modulus));
|
||||
}
|
||||
|
||||
void _bgc_fp64_turn3_normalize(BGC_FP64_Turn3* turn, const double square_modulus)
|
||||
|
|
@ -95,7 +95,7 @@ void _bgc_fp64_turn3_normalize(BGC_FP64_Turn3* turn, const double square_modulus
|
|||
return;
|
||||
}
|
||||
|
||||
bgc_fp64_quaternion_multiply(&turn->_versor, &turn->_versor, sqrt(1.0 / square_modulus));
|
||||
bgc_fp64_quaternion_multiply_by_number(&turn->_versor, &turn->_versor, sqrt(1.0 / square_modulus));
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -289,190 +289,246 @@ int bgc_fp64_turn3_find_direction_difference(BGC_FP64_Turn3* difference, const B
|
|||
return _bgc_fp64_turn3_make_direction_turn(difference, start, end, start_square_modulus * end_square_modulus);
|
||||
}
|
||||
|
||||
// =============== Set Directions =============== //
|
||||
// ============ Make Orthogonal Pair ============ //
|
||||
|
||||
static int _bgc_fp32_turn3_validate_basis(const float primary_square_modulus, const float auxiliary_square_modulus, const float orthogonal_square_modulus)
|
||||
static inline int _bgc_fp32_turn3_get_orthogonal_pair(BGC_FP32_Vector3* unit_main, BGC_FP32_Vector3* unit_branch, const BGC_FP32_Vector3* main, const BGC_FP32_Vector3* branch)
|
||||
{
|
||||
if (primary_square_modulus <= BGC_FP32_SQUARE_EPSILON) {
|
||||
//TODO: add error code for: primary_vector is zero
|
||||
return BGC_FAILED;
|
||||
const float main_square_modulus = bgc_fp32_vector3_get_square_modulus(main);
|
||||
|
||||
if (main_square_modulus <= BGC_FP32_SQUARE_EPSILON) {
|
||||
return _BGC_ERROR_TURN3_EMPTY_MAIN;
|
||||
}
|
||||
|
||||
if (auxiliary_square_modulus <= BGC_FP32_SQUARE_EPSILON) {
|
||||
//TODO: add error code for: auxiliary_vector is zero
|
||||
return BGC_FAILED;
|
||||
const float branch_square_modulus = bgc_fp32_vector3_get_square_modulus(branch);
|
||||
|
||||
if (branch_square_modulus <= BGC_FP32_SQUARE_EPSILON) {
|
||||
return _BGC_ERROR_TURN3_EMPTY_BRANCH;
|
||||
}
|
||||
|
||||
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;
|
||||
bgc_fp32_vector3_multiply(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_square_modulus(unit_branch);
|
||||
|
||||
if (orthogonal_square_modulus <= BGC_FP32_SQUARE_EPSILON) {
|
||||
return _BGC_ERROR_TURN3_PAIR_PARALLEL;
|
||||
}
|
||||
|
||||
bgc_fp32_vector3_multiply(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* unit_main, BGC_FP64_Vector3* unit_branch, const BGC_FP64_Vector3* main, const BGC_FP64_Vector3* branch)
|
||||
{
|
||||
const double main_square_modulus = bgc_fp64_vector3_get_square_modulus(main);
|
||||
|
||||
if (main_square_modulus <= BGC_FP64_SQUARE_EPSILON) {
|
||||
return _BGC_ERROR_TURN3_EMPTY_MAIN;
|
||||
}
|
||||
|
||||
const double branch_square_modulus = bgc_fp64_vector3_get_square_modulus(branch);
|
||||
|
||||
if (branch_square_modulus <= BGC_FP64_SQUARE_EPSILON) {
|
||||
return _BGC_ERROR_TURN3_EMPTY_BRANCH;
|
||||
}
|
||||
|
||||
bgc_fp64_vector3_multiply(unit_main, main, sqrtf(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_square_modulus(unit_branch);
|
||||
|
||||
if (orthogonal_square_modulus <= BGC_FP64_SQUARE_EPSILON) {
|
||||
return _BGC_ERROR_TURN3_PAIR_PARALLEL;
|
||||
}
|
||||
|
||||
bgc_fp64_vector3_multiply(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* quaternion, const BGC_FP32_Vector3* unit_start, const BGC_FP32_Vector3* unit_end, const BGC_FP32_Vector3* 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_square_modulus(&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->s0 = 1.0f;
|
||||
quaternion->x1 = 0.0f;
|
||||
quaternion->x2 = 0.0f;
|
||||
quaternion->x3 = 0.0f;
|
||||
return;
|
||||
}
|
||||
|
||||
// unit_start and unit_end are opposite, angle = 180 degrees
|
||||
quaternion->s0 = 0.0f;
|
||||
quaternion->x1 = unit_orthogonal->x1;
|
||||
quaternion->x2 = unit_orthogonal->x2;
|
||||
quaternion->x3 = unit_orthogonal->x3;
|
||||
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->s0 = cosf(angle);
|
||||
quaternion->x1 = axis.x1 * multiplier;
|
||||
quaternion->x2 = axis.x2 * multiplier;
|
||||
quaternion->x3 = axis.x3 * multiplier;
|
||||
}
|
||||
|
||||
static inline void _bgc_fp64_turn3_get_turning_quaternion(BGC_FP64_Quaternion* quaternion, const BGC_FP64_Vector3* unit_start, const BGC_FP64_Vector3* unit_end, const BGC_FP64_Vector3* 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_square_modulus(&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->s0 = 1.0;
|
||||
quaternion->x1 = 0.0;
|
||||
quaternion->x2 = 0.0;
|
||||
quaternion->x3 = 0.0;
|
||||
return;
|
||||
}
|
||||
|
||||
// unit_start and unit_end are opposite, angle = 180 degrees
|
||||
quaternion->s0 = 0.0;
|
||||
quaternion->x1 = unit_orthogonal->x1;
|
||||
quaternion->x2 = unit_orthogonal->x2;
|
||||
quaternion->x3 = unit_orthogonal->x3;
|
||||
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->s0 = cos(angle);
|
||||
quaternion->x1 = axis.x1 * multiplier;
|
||||
quaternion->x2 = axis.x2 * multiplier;
|
||||
quaternion->x3 = axis.x3 * multiplier;
|
||||
}
|
||||
|
||||
// ============ Make Pair Difference ============ //
|
||||
|
||||
int bgc_fp32_turn3_find_pair_difference(
|
||||
BGC_FP32_Turn3* turn,
|
||||
const BGC_FP32_Vector3* first_pair_main,
|
||||
const BGC_FP32_Vector3* first_pair_branch,
|
||||
const BGC_FP32_Vector3* second_pair_main,
|
||||
const BGC_FP32_Vector3* 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_quaternion_turn_vector_roughly(&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)
|
||||
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);
|
||||
}
|
||||
|
||||
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;
|
||||
int bgc_fp64_turn3_find_pair_difference(
|
||||
BGC_FP64_Turn3* turn,
|
||||
const BGC_FP64_Vector3* first_pair_main,
|
||||
const BGC_FP64_Vector3* first_pair_branch,
|
||||
const BGC_FP64_Vector3* second_pair_main,
|
||||
const BGC_FP64_Vector3* 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;
|
||||
}
|
||||
|
||||
if (auxiliary_square_modulus <= BGC_FP64_SQUARE_EPSILON) {
|
||||
//TODO: add error code for: auxiliary_vector is zero
|
||||
return BGC_FAILED;
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
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_quaternion_turn_vector_roughly(&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)
|
||||
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);
|
||||
}
|
||||
|
||||
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)
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue