Переделка методов turn3_find_direction_difference, возврат функций матриц поворота для turn3
This commit is contained in:
parent
2ce4b64ca3
commit
78d1661c5d
2 changed files with 198 additions and 106 deletions
|
|
@ -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