Добавление функций определения поворотов (versor) между направлениями и базисами
This commit is contained in:
parent
e6a94ab8d9
commit
2a4d5522d3
12 changed files with 784 additions and 287 deletions
|
|
@ -151,6 +151,274 @@ void bgc_versor_set_turn_fp64(const double x1, const double x2, const double x3,
|
|||
bgc_versor_set_values_fp64(cos(half_angle), x1 * multiplier, x2 * multiplier, x3 * multiplier, result);
|
||||
}
|
||||
|
||||
// ========= Make Direction Difference ========== //
|
||||
|
||||
inline int _bgc_versor_make_direction_turn_fp32(const BgcVector3FP32* start, const BgcVector3FP32* end, const float square_modulus_product, BgcVersorFP32* result)
|
||||
{
|
||||
BgcVector3FP32 orthogonal_axis;
|
||||
|
||||
bgc_vector3_get_cross_product_fp32(start, end, &orthogonal_axis);
|
||||
|
||||
const float scalar_product = bgc_vector3_get_scalar_product_fp32(start, end);
|
||||
const float square_modulus = bgc_vector3_get_square_modulus_fp32(&orthogonal_axis);
|
||||
const float square_sine = square_modulus / square_modulus_product;
|
||||
|
||||
if (square_sine > BGC_SQUARE_EPSYLON_FP32) {
|
||||
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_versor_set_values_fp32(cosf(angle), orthogonal_axis.x1 * multiplier, orthogonal_axis.x2 * multiplier, orthogonal_axis.x3 * multiplier, result);
|
||||
return BGC_SOME_TURN;
|
||||
}
|
||||
|
||||
if (scalar_product < 0.0f) {
|
||||
return BGC_OPPOSITE;
|
||||
}
|
||||
|
||||
bgc_versor_reset_fp32(result);
|
||||
return BGC_ZERO_TURN;
|
||||
}
|
||||
|
||||
inline int _bgc_versor_make_direction_turn_fp64(const BgcVector3FP64* start, const BgcVector3FP64* end, const double square_modulus_product, BgcVersorFP64* result)
|
||||
{
|
||||
BgcVector3FP64 orthogonal_axis;
|
||||
|
||||
bgc_vector3_get_cross_product_fp64(start, end, &orthogonal_axis);
|
||||
|
||||
const double scalar_product = bgc_vector3_get_scalar_product_fp64(start, end);
|
||||
const double square_modulus = bgc_vector3_get_square_modulus_fp64(&orthogonal_axis);
|
||||
const double square_sine = square_modulus / square_modulus_product;
|
||||
|
||||
if (square_sine > BGC_SQUARE_EPSYLON_FP64) {
|
||||
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_versor_set_values_fp64(cos(angle), orthogonal_axis.x1 * multiplier, orthogonal_axis.x2 * multiplier, orthogonal_axis.x3 * multiplier, result);
|
||||
return BGC_SOME_TURN;
|
||||
}
|
||||
|
||||
if (scalar_product < 0.0) {
|
||||
return BGC_OPPOSITE;
|
||||
}
|
||||
|
||||
bgc_versor_reset_fp64(result);
|
||||
return BGC_ZERO_TURN;
|
||||
}
|
||||
|
||||
int bgc_versor_make_direction_difference_fp32(const BgcVector3FP32* start, const BgcVector3FP32* end, BgcVersorFP32* result)
|
||||
{
|
||||
const float start_square_modulus = bgc_vector3_get_square_modulus_fp32(start);
|
||||
const float end_square_modulus = bgc_vector3_get_square_modulus_fp32(end);
|
||||
|
||||
if (start_square_modulus <= BGC_SQUARE_EPSYLON_FP32 || end_square_modulus <= BGC_SQUARE_EPSYLON_FP32) {
|
||||
bgc_versor_reset_fp32(result);
|
||||
return BGC_ZERO_TURN;
|
||||
}
|
||||
|
||||
return _bgc_versor_make_direction_turn_fp32(start, end, start_square_modulus * end_square_modulus, result);
|
||||
}
|
||||
|
||||
int bgc_versor_make_direction_difference_fp64(const BgcVector3FP64* start, const BgcVector3FP64* end, BgcVersorFP64* result)
|
||||
{
|
||||
const double start_square_modulus = bgc_vector3_get_square_modulus_fp64(start);
|
||||
const double end_square_modulus = bgc_vector3_get_square_modulus_fp64(end);
|
||||
|
||||
if (start_square_modulus <= BGC_SQUARE_EPSYLON_FP64 || end_square_modulus <= BGC_SQUARE_EPSYLON_FP64) {
|
||||
bgc_versor_reset_fp64(result);
|
||||
return BGC_ZERO_TURN;
|
||||
}
|
||||
|
||||
return _bgc_versor_make_direction_turn_fp64(start, end, start_square_modulus * end_square_modulus, result);
|
||||
}
|
||||
|
||||
// =============== Set Directions =============== //
|
||||
|
||||
inline int _bgc_versor_validate_basis_fp32(const float primary_square_modulus, const float auxiliary_square_modulus, const float orthogonal_square_modulus)
|
||||
{
|
||||
if (primary_square_modulus <= BGC_SQUARE_EPSYLON_FP32) {
|
||||
//TODO: add error code for: primary_vector is zero
|
||||
return BGC_FAILED;
|
||||
}
|
||||
|
||||
if (auxiliary_square_modulus <= BGC_SQUARE_EPSYLON_FP32) {
|
||||
//TODO: add error code for: auxiliary_vector is zero
|
||||
return BGC_FAILED;
|
||||
}
|
||||
|
||||
if (orthogonal_square_modulus / (primary_square_modulus * auxiliary_square_modulus) <= BGC_SQUARE_EPSYLON_FP32) {
|
||||
//TODO: add error code for: primary_vector and auxiliary_vector are parallel
|
||||
return BGC_FAILED;
|
||||
}
|
||||
|
||||
return BGC_SUCCESS;
|
||||
}
|
||||
|
||||
inline int _bgc_versor_validate_basis_fp64(const double primary_square_modulus, const double auxiliary_square_modulus, const double orthogonal_square_modulus)
|
||||
{
|
||||
if (primary_square_modulus <= BGC_SQUARE_EPSYLON_FP64) {
|
||||
//TODO: add error code for: primary_vector is zero
|
||||
return BGC_FAILED;
|
||||
}
|
||||
|
||||
if (auxiliary_square_modulus <= BGC_SQUARE_EPSYLON_FP64) {
|
||||
//TODO: add error code for: auxiliary_vector is zero
|
||||
return BGC_FAILED;
|
||||
}
|
||||
|
||||
if (orthogonal_square_modulus / (primary_square_modulus * auxiliary_square_modulus) <= BGC_SQUARE_EPSYLON_FP64) {
|
||||
//TODO: add error code for: primary_vector and auxiliary_vector are parallel
|
||||
return BGC_FAILED;
|
||||
}
|
||||
|
||||
return BGC_SUCCESS;
|
||||
}
|
||||
|
||||
int bgc_versor_make_basis_difference_fp32(
|
||||
const BgcVector3FP32* initial_primary_direction,
|
||||
const BgcVector3FP32* initial_auxiliary_direction,
|
||||
const BgcVector3FP32* final_primary_direction,
|
||||
const BgcVector3FP32* final_auxiliary_direction,
|
||||
BgcVersorFP32* result
|
||||
)
|
||||
{
|
||||
BgcVector3FP32 initial_orthogonal_direction, turned_orthogonal_direction, final_orthogonal_direction;
|
||||
|
||||
// Step 1: Validate initial basis:
|
||||
bgc_vector3_get_cross_product_fp32(initial_primary_direction, initial_auxiliary_direction, &initial_orthogonal_direction);
|
||||
|
||||
const float initial_primary_square_modulus = bgc_vector3_get_square_modulus_fp32(initial_primary_direction);
|
||||
const float initial_auxiliary_square_modulus = bgc_vector3_get_square_modulus_fp32(initial_auxiliary_direction);
|
||||
const float initial_orthogonal_square_modulus = bgc_vector3_get_square_modulus_fp32(&initial_orthogonal_direction);
|
||||
|
||||
const int initial_basis_valudation = _bgc_versor_validate_basis_fp32(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_vector3_get_cross_product_fp32(final_primary_direction, final_auxiliary_direction, &final_orthogonal_direction);
|
||||
|
||||
const float final_primary_square_modulus = bgc_vector3_get_square_modulus_fp32(final_primary_direction);
|
||||
const float final_auxiliary_square_modulus = bgc_vector3_get_square_modulus_fp32(final_auxiliary_direction);
|
||||
const float final_orthogonal_square_modulus = bgc_vector3_get_square_modulus_fp32(&final_orthogonal_direction);
|
||||
|
||||
const int final_basis_valudation = _bgc_versor_validate_basis_fp32(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_vector3_divide_fp32(&initial_orthogonal_direction, sqrtf(initial_orthogonal_square_modulus), &initial_orthogonal_direction);
|
||||
bgc_vector3_divide_fp32(&final_orthogonal_direction, sqrtf(final_orthogonal_square_modulus), &final_orthogonal_direction);
|
||||
|
||||
BgcVersorFP32 turn1, turn2;
|
||||
|
||||
// Step 4: Find turn1
|
||||
int turn1_code = _bgc_versor_make_direction_turn_fp32(initial_primary_direction, final_primary_direction, initial_primary_square_modulus * final_primary_square_modulus, &turn1);
|
||||
|
||||
if (turn1_code == BGC_OPPOSITE) {
|
||||
bgc_versor_set_values_fp32(0.0f, initial_orthogonal_direction.x1, initial_orthogonal_direction.x2, initial_orthogonal_direction.x3, &turn1);
|
||||
}
|
||||
|
||||
bgc_versor_turn_vector_fp32(&turn1, &initial_orthogonal_direction, &turned_orthogonal_direction);
|
||||
|
||||
// Step 5: Find turn2:
|
||||
int turn2_code = _bgc_versor_make_direction_turn_fp32(&turned_orthogonal_direction, &final_orthogonal_direction, 1.0f, &turn2);
|
||||
|
||||
if (turn2_code == BGC_OPPOSITE) {
|
||||
const float turn2_multiplier = sqrtf(1.0f / final_primary_square_modulus);
|
||||
|
||||
bgc_versor_set_values_fp32(0.0f,
|
||||
final_primary_direction->x1 * turn2_multiplier,
|
||||
final_primary_direction->x2 * turn2_multiplier,
|
||||
final_primary_direction->x3 * turn2_multiplier,
|
||||
&turn2
|
||||
);
|
||||
}
|
||||
|
||||
// Step 6: Combine turn1 and turn2:
|
||||
bgc_versor_combine_fp32(&turn2, &turn1, result);
|
||||
|
||||
return BGC_SUCCESS;
|
||||
}
|
||||
|
||||
int bgc_versor_make_basis_difference_fp64(
|
||||
const BgcVector3FP64* initial_primary_direction,
|
||||
const BgcVector3FP64* initial_auxiliary_direction,
|
||||
const BgcVector3FP64* final_primary_direction,
|
||||
const BgcVector3FP64* final_auxiliary_direction,
|
||||
BgcVersorFP64* result
|
||||
)
|
||||
{
|
||||
BgcVector3FP64 initial_orthogonal_direction, turned_orthogonal_direction, final_orthogonal_direction;
|
||||
|
||||
// Step 1: Validate initial basis:
|
||||
bgc_vector3_get_cross_product_fp64(initial_primary_direction, initial_auxiliary_direction, &initial_orthogonal_direction);
|
||||
|
||||
const double initial_primary_square_modulus = bgc_vector3_get_square_modulus_fp64(initial_primary_direction);
|
||||
const double initial_auxiliary_square_modulus = bgc_vector3_get_square_modulus_fp64(initial_auxiliary_direction);
|
||||
const double initial_orthogonal_square_modulus = bgc_vector3_get_square_modulus_fp64(&initial_orthogonal_direction);
|
||||
|
||||
const int initial_basis_valudation = _bgc_versor_validate_basis_fp64(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_vector3_get_cross_product_fp64(final_primary_direction, final_auxiliary_direction, &final_orthogonal_direction);
|
||||
|
||||
const double final_primary_square_modulus = bgc_vector3_get_square_modulus_fp64(final_primary_direction);
|
||||
const double final_auxiliary_square_modulus = bgc_vector3_get_square_modulus_fp64(final_auxiliary_direction);
|
||||
const double final_orthogonal_square_modulus = bgc_vector3_get_square_modulus_fp64(&final_orthogonal_direction);
|
||||
|
||||
const int final_basis_valudation = _bgc_versor_validate_basis_fp64(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_vector3_divide_fp64(&initial_orthogonal_direction, sqrt(initial_orthogonal_square_modulus), &initial_orthogonal_direction);
|
||||
bgc_vector3_divide_fp64(&final_orthogonal_direction, sqrt(final_orthogonal_square_modulus), &final_orthogonal_direction);
|
||||
|
||||
BgcVersorFP64 turn1, turn2;
|
||||
|
||||
// Step 4: Find turn1
|
||||
int turn1_code = _bgc_versor_make_direction_turn_fp64(initial_primary_direction, final_primary_direction, initial_primary_square_modulus * final_primary_square_modulus, &turn1);
|
||||
|
||||
if (turn1_code == BGC_OPPOSITE) {
|
||||
bgc_versor_set_values_fp64(0.0, initial_orthogonal_direction.x1, initial_orthogonal_direction.x2, initial_orthogonal_direction.x3, &turn1);
|
||||
}
|
||||
|
||||
bgc_versor_turn_vector_fp64(&turn1, &initial_orthogonal_direction, &turned_orthogonal_direction);
|
||||
|
||||
// Step 5: Find turn2:
|
||||
int turn2_code = _bgc_versor_make_direction_turn_fp64(&turned_orthogonal_direction, &final_orthogonal_direction, 1.0f, &turn2);
|
||||
|
||||
if (turn2_code == BGC_OPPOSITE) {
|
||||
const double turn2_multiplier = sqrt(1.0 / final_primary_square_modulus);
|
||||
|
||||
bgc_versor_set_values_fp64(0.0,
|
||||
final_primary_direction->x1 * turn2_multiplier,
|
||||
final_primary_direction->x2 * turn2_multiplier,
|
||||
final_primary_direction->x3 * turn2_multiplier,
|
||||
&turn2
|
||||
);
|
||||
}
|
||||
|
||||
// Step 6: Combine turn1 and turn2:
|
||||
bgc_versor_combine_fp64(&turn2, &turn1, result);
|
||||
|
||||
return BGC_SUCCESS;
|
||||
}
|
||||
|
||||
// =============== Get Exponation =============== //
|
||||
|
||||
void bgc_versor_get_exponation_fp32(const BgcVersorFP32* base, const float exponent, BgcVersorFP32* power)
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue