Переделка методов turn3_find_direction_difference, возврат функций матриц поворота для turn3
This commit is contained in:
parent
2ce4b64ca3
commit
78d1661c5d
2 changed files with 198 additions and 106 deletions
|
|
@ -201,92 +201,95 @@ void bgc_fp64_turn3_set_rotation(BGC_FP64_Turn3* turn, const double x1, const do
|
|||
}
|
||||
}
|
||||
|
||||
// ========= Make Direction Difference ========== //
|
||||
// ========= Find 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_product)
|
||||
int bgc_fp32_turn3_find_direction_difference(BGC_FP32_Turn3* turn, const BGC_FP32_Vector3* first, const BGC_FP32_Vector3* second)
|
||||
{
|
||||
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_FP32_SQUARE_EPSILON) {
|
||||
const float cosine = scalar_product / sqrtf(square_modulus_product);
|
||||
const float angle = 0.5f * atan2f(sqrtf(square_sine), cosine);
|
||||
|
||||
const float multiplier = sinf(angle) * sqrtf(1.0f / square_modulus);
|
||||
|
||||
bgc_fp32_turn3_set_raw_values(turn, 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;
|
||||
}
|
||||
const float first_square_modulus = bgc_fp32_vector3_get_square_modulus(first);
|
||||
|
||||
bgc_fp32_turn3_reset(turn);
|
||||
|
||||
return BGC_ZERO_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_square_modulus(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_square_modulus(&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;
|
||||
}
|
||||
|
||||
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_raw_values(turn, cosf(angle), axis.x1 * vector_multiplier, axis.x2 * vector_multiplier, axis.x3 * vector_multiplier);
|
||||
|
||||
return BGC_SUCCESS;
|
||||
}
|
||||
|
||||
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)
|
||||
int bgc_fp64_turn3_find_direction_difference(BGC_FP64_Turn3* turn, const BGC_FP64_Vector3* first, const BGC_FP64_Vector3* second)
|
||||
{
|
||||
BGC_FP64_Vector3 orthogonal_axis;
|
||||
const double first_square_modulus = bgc_fp64_vector3_get_square_modulus(first);
|
||||
|
||||
bgc_fp64_vector3_get_cross_product(&orthogonal_axis, start, end);
|
||||
bgc_fp64_turn3_reset(turn);
|
||||
|
||||
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 (first_square_modulus <= BGC_FP64_SQUARE_EPSILON) {
|
||||
return BGC_ERROR_TURN3_FIRST_VECTOR_ZERO;
|
||||
}
|
||||
|
||||
if (scalar_product < 0.0) {
|
||||
return BGC_OPPOSITE;
|
||||
const double second_square_modulus = bgc_fp64_vector3_get_square_modulus(second);
|
||||
|
||||
if (second_square_modulus <= BGC_FP64_SQUARE_EPSILON) {
|
||||
return BGC_ERROR_TURN3_SECOND_VECTOR_ZERO;
|
||||
}
|
||||
|
||||
bgc_fp64_turn3_reset(versor);
|
||||
BGC_FP64_Vector3 axis;
|
||||
|
||||
return BGC_ZERO_TURN;
|
||||
}
|
||||
bgc_fp64_vector3_get_cross_product(&axis, first, second);
|
||||
|
||||
// ========= Find Direction Difference ========== //
|
||||
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_square_modulus(&axis);
|
||||
|
||||
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;
|
||||
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_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_SUCCESS;
|
||||
}
|
||||
|
||||
return _bgc_fp64_turn3_make_direction_turn(difference, start, end, start_square_modulus * end_square_modulus);
|
||||
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_raw_values(turn, cos(angle), axis.x1 * vector_multiplier, axis.x2 * vector_multiplier, axis.x3 * vector_multiplier);
|
||||
|
||||
return BGC_SUCCESS;
|
||||
}
|
||||
|
||||
// ============ Make Orthogonal Pair ============ //
|
||||
|
|
@ -334,7 +337,7 @@ static inline int _bgc_fp64_turn3_get_orthogonal_pair(BGC_FP64_Vector3* unit_mai
|
|||
return _BGC_ERROR_TURN3_EMPTY_BRANCH;
|
||||
}
|
||||
|
||||
bgc_fp64_vector3_multiply(unit_main, main, sqrtf(1.0 / main_square_modulus));
|
||||
bgc_fp64_vector3_multiply(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));
|
||||
|
||||
|
|
|
|||
|
|
@ -9,32 +9,23 @@
|
|||
#include "matrix3x3.h"
|
||||
#include "quaternion.h"
|
||||
|
||||
#define BGC_SOME_TURN 1
|
||||
#define BGC_ZERO_TURN 0
|
||||
#define BGC_OPPOSITE -1
|
||||
#define BGC_ERROR_TURN3_FIRST_VECTOR_ZERO -3010
|
||||
#define BGC_ERROR_TURN3_SECOND_VECTOR_ZERO -3011
|
||||
#define BGC_ERROR_TURN3_VECTORS_OPPOSITE -3012
|
||||
|
||||
#define _BGC_ERROR_TURN3_FIRST_PAIR 3000
|
||||
#define _BGC_ERROR_TURN3_SECOND_PAIR 3010
|
||||
#define _BGC_ERROR_TURN3_EMPTY_MAIN 1
|
||||
#define _BGC_ERROR_TURN3_EMPTY_BRANCH 2
|
||||
#define _BGC_ERROR_TURN3_PAIR_PARALLEL 3
|
||||
#define _BGC_ERROR_TURN3_FIRST_PAIR -3020
|
||||
#define _BGC_ERROR_TURN3_SECOND_PAIR -3030
|
||||
#define _BGC_ERROR_TURN3_EMPTY_MAIN -1
|
||||
#define _BGC_ERROR_TURN3_EMPTY_BRANCH -2
|
||||
#define _BGC_ERROR_TURN3_PAIR_PARALLEL -3
|
||||
|
||||
#define BGC_ERROR_TURN3_FIRST_PAIR_EMPTY_MAIN 3001
|
||||
#define BGC_ERROR_TURN3_FIRST_PAIR_EMPTY_BRANCH 3002
|
||||
#define BGC_ERROR_TURN3_FIRST_PAIR_PARALLEL 3003
|
||||
#define BGC_ERROR_TURN3_FIRST_PAIR_ZERO_MAIN -3021
|
||||
#define BGC_ERROR_TURN3_FIRST_PAIR_ZERO_BRANCH -3022
|
||||
#define BGC_ERROR_TURN3_FIRST_PAIR_PARALLEL -3023
|
||||
|
||||
#define BGC_ERROR_TURN3_SECOND_PAIR_EMPTY_MAIN 3011
|
||||
#define BGC_ERROR_TURN3_SECOND_PAIR_EMPTY_BRANCH 3012
|
||||
#define BGC_ERROR_TURN3_SECOND_PAIR_PARALLEL 3013
|
||||
|
||||
#define BGC_ERROR_PRIMARY_DIRECTION_UNKNOWN -3001
|
||||
#define BGC_ERROR_PRIMARY_VECTOR_IS_ZERO -3002
|
||||
|
||||
#define BGC_ERROR_AUXILIARY_DIRECTION_UNKNOWN -3011
|
||||
#define BGC_ERROR_AUXILIARY_VECTOR_IS_ZERO -3012
|
||||
|
||||
#define BGC_ERROR_DIRECTIONS_PARALLEL -3021
|
||||
#define BGC_ERROR_VECTORS_PARALLEL -3022
|
||||
#define BGC_ERROR_TURN3_SECOND_PAIR_ZERO_MAIN -3031
|
||||
#define BGC_ERROR_TURN3_SECOND_PAIR_ZERO_BRANCH -3032
|
||||
#define BGC_ERROR_TURN3_SECOND_PAIR_PARALLEL -3033
|
||||
|
||||
// =================== Types ==================== //
|
||||
|
||||
|
|
@ -143,9 +134,9 @@ void bgc_fp64_turn3_set_rotation(BGC_FP64_Turn3* turn, const double x1, const do
|
|||
|
||||
// ========= Find Direction Difference ========== //
|
||||
|
||||
int bgc_fp32_turn3_find_direction_difference(BGC_FP32_Turn3* difference, const BGC_FP32_Vector3* start, const BGC_FP32_Vector3* end);
|
||||
int bgc_fp32_turn3_find_direction_difference(BGC_FP32_Turn3* turn, const BGC_FP32_Vector3* start, const BGC_FP32_Vector3* end);
|
||||
|
||||
int bgc_fp64_turn3_find_direction_difference(BGC_FP64_Turn3* difference, const BGC_FP64_Vector3* start, const BGC_FP64_Vector3* end);
|
||||
int bgc_fp64_turn3_find_direction_difference(BGC_FP64_Turn3* turn, const BGC_FP64_Vector3* start, const BGC_FP64_Vector3* end);
|
||||
|
||||
// ======= Find Direction Pair Difference ======= //
|
||||
|
||||
|
|
@ -401,36 +392,134 @@ void bgc_fp64_turn3_spherically_interpolate(BGC_FP64_Turn3* interpolation, const
|
|||
|
||||
inline void bgc_fp32_turn3_get_rotation_matrix(BGC_FP32_Matrix3x3* matrix, const BGC_FP32_Turn3* turn)
|
||||
{
|
||||
bgc_fp32_quaternion_get_rotation_matrix(matrix, &turn->_versor);
|
||||
const float s0s0 = turn->_versor.s0 * turn->_versor.s0;
|
||||
const float x1x1 = turn->_versor.x1 * turn->_versor.x1;
|
||||
const float x2x2 = turn->_versor.x2 * turn->_versor.x2;
|
||||
const float x3x3 = turn->_versor.x3 * turn->_versor.x3;
|
||||
|
||||
const float s0x1 = turn->_versor.s0 * turn->_versor.x1;
|
||||
const float s0x2 = turn->_versor.s0 * turn->_versor.x2;
|
||||
const float s0x3 = turn->_versor.s0 * turn->_versor.x3;
|
||||
|
||||
const float x1x2 = turn->_versor.x1 * turn->_versor.x2;
|
||||
const float x1x3 = turn->_versor.x1 * turn->_versor.x3;
|
||||
|
||||
const float x2x3 = turn->_versor.x2 * turn->_versor.x3;
|
||||
|
||||
matrix->r1c1 = ((s0s0 + x1x1) - (x2x2 + x3x3));
|
||||
matrix->r2c2 = ((s0s0 + x2x2) - (x1x1 + x3x3));
|
||||
matrix->r3c3 = ((s0s0 + x3x3) - (x1x1 + x2x2));
|
||||
|
||||
matrix->r1c2 = 2.0f * (x1x2 - s0x3);
|
||||
matrix->r2c3 = 2.0f * (x2x3 - s0x1);
|
||||
matrix->r3c1 = 2.0f * (x1x3 - s0x2);
|
||||
|
||||
matrix->r2c1 = 2.0f * (x1x2 + s0x3);
|
||||
matrix->r3c2 = 2.0f * (x2x3 + s0x1);
|
||||
matrix->r1c3 = 2.0f * (x1x3 + s0x2);
|
||||
}
|
||||
|
||||
inline void bgc_fp64_turn3_get_rotation_matrix(BGC_FP64_Matrix3x3* matrix, const BGC_FP64_Turn3* turn)
|
||||
{
|
||||
bgc_fp64_quaternion_get_rotation_matrix(matrix, &turn->_versor);
|
||||
const double s0s0 = turn->_versor.s0 * turn->_versor.s0;
|
||||
const double x1x1 = turn->_versor.x1 * turn->_versor.x1;
|
||||
const double x2x2 = turn->_versor.x2 * turn->_versor.x2;
|
||||
const double x3x3 = turn->_versor.x3 * turn->_versor.x3;
|
||||
|
||||
const double s0x1 = turn->_versor.s0 * turn->_versor.x1;
|
||||
const double s0x2 = turn->_versor.s0 * turn->_versor.x2;
|
||||
const double s0x3 = turn->_versor.s0 * turn->_versor.x3;
|
||||
|
||||
const double x1x2 = turn->_versor.x1 * turn->_versor.x2;
|
||||
const double x1x3 = turn->_versor.x1 * turn->_versor.x3;
|
||||
|
||||
const double x2x3 = turn->_versor.x2 * turn->_versor.x3;
|
||||
|
||||
matrix->r1c1 = ((s0s0 + x1x1) - (x2x2 + x3x3));
|
||||
matrix->r2c2 = ((s0s0 + x2x2) - (x1x1 + x3x3));
|
||||
matrix->r3c3 = ((s0s0 + x3x3) - (x1x1 + x2x2));
|
||||
|
||||
matrix->r1c2 = 2.0 * (x1x2 - s0x3);
|
||||
matrix->r2c3 = 2.0 * (x2x3 - s0x1);
|
||||
matrix->r3c1 = 2.0 * (x1x3 - s0x2);
|
||||
|
||||
matrix->r2c1 = 2.0 * (x1x2 + s0x3);
|
||||
matrix->r3c2 = 2.0 * (x2x3 + s0x1);
|
||||
matrix->r1c3 = 2.0 * (x1x3 + s0x2);
|
||||
}
|
||||
|
||||
// ============= Get Reverse Matrix ============= //
|
||||
|
||||
inline void bgc_fp32_turn3_get_reverse_matrix(BGC_FP32_Matrix3x3* matrix, const BGC_FP32_Turn3* turn)
|
||||
{
|
||||
bgc_fp32_quaternion_get_reverse_matrix(matrix, &turn->_versor);
|
||||
const float s0s0 = turn->_versor.s0 * turn->_versor.s0;
|
||||
const float x1x1 = turn->_versor.x1 * turn->_versor.x1;
|
||||
const float x2x2 = turn->_versor.x2 * turn->_versor.x2;
|
||||
const float x3x3 = turn->_versor.x3 * turn->_versor.x3;
|
||||
|
||||
const float s0x1 = turn->_versor.s0 * turn->_versor.x1;
|
||||
const float s0x2 = turn->_versor.s0 * turn->_versor.x2;
|
||||
const float s0x3 = turn->_versor.s0 * turn->_versor.x3;
|
||||
|
||||
const float x1x2 = turn->_versor.x1 * turn->_versor.x2;
|
||||
const float x1x3 = turn->_versor.x1 * turn->_versor.x3;
|
||||
|
||||
const float x2x3 = turn->_versor.x2 * turn->_versor.x3;
|
||||
|
||||
matrix->r1c1 = ((s0s0 + x1x1) - (x2x2 + x3x3));
|
||||
matrix->r2c2 = ((s0s0 + x2x2) - (x1x1 + x3x3));
|
||||
matrix->r3c3 = ((s0s0 + x3x3) - (x1x1 + x2x2));
|
||||
|
||||
matrix->r1c2 = 2.0f * (x1x2 + s0x3);
|
||||
matrix->r2c3 = 2.0f * (x2x3 + s0x1);
|
||||
matrix->r3c1 = 2.0f * (x1x3 + s0x2);
|
||||
|
||||
matrix->r2c1 = 2.0f * (x1x2 - s0x3);
|
||||
matrix->r3c2 = 2.0f * (x2x3 - s0x1);
|
||||
matrix->r1c3 = 2.0f * (x1x3 - s0x2);
|
||||
}
|
||||
|
||||
inline void bgc_fp64_turn3_get_reverse_matrix(BGC_FP64_Matrix3x3* matrix, const BGC_FP64_Turn3* turn)
|
||||
{
|
||||
bgc_fp64_quaternion_get_reverse_matrix(matrix, &turn->_versor);
|
||||
const double s0s0 = turn->_versor.s0 * turn->_versor.s0;
|
||||
const double x1x1 = turn->_versor.x1 * turn->_versor.x1;
|
||||
const double x2x2 = turn->_versor.x2 * turn->_versor.x2;
|
||||
const double x3x3 = turn->_versor.x3 * turn->_versor.x3;
|
||||
|
||||
const double s0x1 = turn->_versor.s0 * turn->_versor.x1;
|
||||
const double s0x2 = turn->_versor.s0 * turn->_versor.x2;
|
||||
const double s0x3 = turn->_versor.s0 * turn->_versor.x3;
|
||||
|
||||
const double x1x2 = turn->_versor.x1 * turn->_versor.x2;
|
||||
const double x1x3 = turn->_versor.x1 * turn->_versor.x3;
|
||||
|
||||
const double x2x3 = turn->_versor.x2 * turn->_versor.x3;
|
||||
|
||||
matrix->r1c1 = ((s0s0 + x1x1) - (x2x2 + x3x3));
|
||||
matrix->r2c2 = ((s0s0 + x2x2) - (x1x1 + x3x3));
|
||||
matrix->r3c3 = ((s0s0 + x3x3) - (x1x1 + x2x2));
|
||||
|
||||
matrix->r1c2 = 2.0 * (x1x2 + s0x3);
|
||||
matrix->r2c3 = 2.0 * (x2x3 + s0x1);
|
||||
matrix->r3c1 = 2.0 * (x1x3 + s0x2);
|
||||
|
||||
matrix->r2c1 = 2.0 * (x1x2 - s0x3);
|
||||
matrix->r3c2 = 2.0 * (x2x3 - s0x1);
|
||||
matrix->r1c3 = 2.0 * (x1x3 - s0x2);
|
||||
}
|
||||
|
||||
// ============= Get Both Matrixes ============== //
|
||||
|
||||
inline void bgc_fp32_turn3_get_both_matrices(BGC_FP32_Matrix3x3* rotation, BGC_FP32_Matrix3x3* reverse, const BGC_FP32_Turn3* turn)
|
||||
{
|
||||
bgc_fp32_quaternion_get_both_matrices(rotation, reverse, &turn->_versor);
|
||||
bgc_fp32_turn3_get_reverse_matrix(reverse, turn);
|
||||
bgc_fp32_matrix3x3_get_transposed(rotation, reverse);
|
||||
}
|
||||
|
||||
inline void bgc_fp64_turn3_get_both_matrices(BGC_FP64_Matrix3x3* rotation, BGC_FP64_Matrix3x3* reverse, const BGC_FP64_Turn3* turn)
|
||||
{
|
||||
bgc_fp64_quaternion_get_both_matrices(rotation, reverse, &turn->_versor);
|
||||
bgc_fp64_turn3_get_reverse_matrix(reverse, turn);
|
||||
bgc_fp64_matrix3x3_get_transposed(rotation, reverse);
|
||||
}
|
||||
|
||||
// ================ Turn Vector ================= //
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue