Добавление базисов как вспомогательной структуры
This commit is contained in:
parent
9d7011e81e
commit
e6a94ab8d9
9 changed files with 345 additions and 14 deletions
232
basic-geometry/basis3.c
Normal file
232
basic-geometry/basis3.c
Normal file
|
|
@ -0,0 +1,232 @@
|
|||
#include <math.h>
|
||||
#include "./basis3.h"
|
||||
|
||||
typedef struct {
|
||||
int e1, e2, e3;
|
||||
} _BgcBasis3State;
|
||||
|
||||
static inline void _bgc_basis3_state_reset(_BgcBasis3State* state)
|
||||
{
|
||||
state->e1 = 0;
|
||||
state->e2 = 0;
|
||||
state->e3 = 0;
|
||||
}
|
||||
|
||||
static inline int _bgc_basis3_validate_directions(const int primary_direction, const int auxiliary_direction)
|
||||
{
|
||||
if (!bgc_is_correct_direction(primary_direction)) {
|
||||
return BGC_ERROR_BASIS3_PRIMARY_DIRECTION_UNKNOWN;
|
||||
}
|
||||
|
||||
if (!bgc_is_correct_direction(auxiliary_direction)) {
|
||||
return BGC_ERROR_BASIS3_AUXILIARY_DIRECTION_UNKNOWN;
|
||||
}
|
||||
|
||||
if (primary_direction == auxiliary_direction || primary_direction == -auxiliary_direction) {
|
||||
return BGC_ERROR_BASIS3_PRIMARY_AUXILIARY_PARALLEL;
|
||||
}
|
||||
|
||||
return BGC_SUCCESS;
|
||||
}
|
||||
|
||||
static inline void _bgc_basis3_load_axis_fp32(const int direction, const BgcVector3FP32* axis, _BgcBasis3State* state, BgcBasis3FP32* basis)
|
||||
{
|
||||
switch (direction) {
|
||||
case BGC_DIRECTION_X1:
|
||||
bgc_vector3_copy_fp32(axis, &basis->e1);
|
||||
state->e1 = 1;
|
||||
break;
|
||||
|
||||
case -BGC_DIRECTION_X1:
|
||||
bgc_vector3_reverse_fp32(axis, &basis->e1);
|
||||
state->e1 = 1;
|
||||
break;
|
||||
|
||||
case BGC_DIRECTION_X2:
|
||||
bgc_vector3_copy_fp32(axis, &basis->e2);
|
||||
state->e2 = 1;
|
||||
break;
|
||||
|
||||
case -BGC_DIRECTION_X2:
|
||||
bgc_vector3_reverse_fp32(axis, &basis->e2);
|
||||
state->e2 = 1;
|
||||
break;
|
||||
|
||||
case BGC_DIRECTION_X3:
|
||||
bgc_vector3_copy_fp32(axis, &basis->e3);
|
||||
state->e3 = 1;
|
||||
break;
|
||||
|
||||
case -BGC_DIRECTION_X3:
|
||||
bgc_vector3_reverse_fp32(axis, &basis->e3);
|
||||
state->e3 = 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
int bgc_basis3_make_from_directions_fp32(
|
||||
const int primary_direction, const BgcVector3FP32* primary_vector,
|
||||
const int auxiliary_direction, const BgcVector3FP32* auxiliary_vector,
|
||||
BgcBasis3FP32* basis
|
||||
) {
|
||||
const int direstion_validation_code = _bgc_basis3_validate_directions(primary_direction, auxiliary_direction);
|
||||
|
||||
if (direstion_validation_code != BGC_SUCCESS) {
|
||||
return direstion_validation_code;
|
||||
}
|
||||
|
||||
const float primary_square_modulus = bgc_vector3_get_square_modulus_fp32(primary_vector);
|
||||
|
||||
if (primary_square_modulus <= BGC_SQUARE_EPSYLON_FP32) {
|
||||
return BGC_ERROR_BASIS3_PRIMARY_VECTOR_IS_ZERO;
|
||||
}
|
||||
|
||||
const float auxiliary_square_modulus = bgc_vector3_get_square_modulus_fp32(auxiliary_vector);
|
||||
|
||||
if (auxiliary_square_modulus <= BGC_SQUARE_EPSYLON_FP32) {
|
||||
return BGC_ERROR_BASIS3_AUXILIARY_VECTOR_IS_ZERO;
|
||||
}
|
||||
|
||||
BgcVector3FP32 orthogonal;
|
||||
|
||||
bgc_vector3_get_cross_product_fp32(primary_vector, auxiliary_vector, &orthogonal);
|
||||
|
||||
const float orthogonal_square_modulus = bgc_vector3_get_square_modulus_fp32(&orthogonal);
|
||||
|
||||
if (orthogonal_square_modulus <= BGC_SQUARE_EPSYLON_FP32 * primary_square_modulus * auxiliary_square_modulus) {
|
||||
return BGC_ERROR_BASIS3_PRIMARY_AUXILIARY_PARALLEL;
|
||||
}
|
||||
|
||||
BgcVector3FP32 primary_axis, auxiliary_axis;
|
||||
|
||||
bgc_vector3_multiply_fp32(primary_vector, sqrtf(1.0f / primary_square_modulus), &primary_axis);
|
||||
|
||||
bgc_vector3_subtract_scaled_fp32(auxiliary_vector, &primary_axis, bgc_vector3_get_scalar_product_fp32(auxiliary_vector, &primary_axis), &auxiliary_axis);
|
||||
|
||||
bgc_vector3_multiply_fp32(&auxiliary_axis, sqrtf(1.0f / bgc_vector3_get_square_modulus_fp32(&auxiliary_axis)), &auxiliary_axis);
|
||||
|
||||
_BgcBasis3State state;
|
||||
|
||||
_bgc_basis3_state_reset(&state);
|
||||
|
||||
_bgc_basis3_load_axis_fp32(primary_direction, &primary_axis, &state, basis);
|
||||
_bgc_basis3_load_axis_fp32(auxiliary_direction, &auxiliary_axis, &state, basis);
|
||||
|
||||
if (!state.e1) {
|
||||
bgc_vector3_get_cross_product_fp32(&basis->e2, &basis->e3, &basis->e1);
|
||||
bgc_vector3_normalize_fp32(&basis->e1);
|
||||
return BGC_SUCCESS;
|
||||
}
|
||||
|
||||
if (!state.e2) {
|
||||
bgc_vector3_get_cross_product_fp32(&basis->e3, &basis->e1, &basis->e2);
|
||||
bgc_vector3_normalize_fp32(&basis->e2);
|
||||
return BGC_SUCCESS;
|
||||
}
|
||||
|
||||
bgc_vector3_get_cross_product_fp32(&basis->e1, &basis->e2, &basis->e3);
|
||||
bgc_vector3_normalize_fp32(&basis->e3);
|
||||
|
||||
return BGC_SUCCESS;
|
||||
}
|
||||
|
||||
static inline void _bgc_basis3_load_axis_fp64(const int direction, const BgcVector3FP64* axis, _BgcBasis3State* state, BgcBasis3FP64* basis)
|
||||
{
|
||||
switch (direction) {
|
||||
case BGC_DIRECTION_X1:
|
||||
bgc_vector3_copy_fp64(axis, &basis->e1);
|
||||
state->e1 = 1;
|
||||
break;
|
||||
|
||||
case -BGC_DIRECTION_X1:
|
||||
bgc_vector3_reverse_fp64(axis, &basis->e1);
|
||||
state->e1 = 1;
|
||||
break;
|
||||
|
||||
case BGC_DIRECTION_X2:
|
||||
bgc_vector3_copy_fp64(axis, &basis->e2);
|
||||
state->e2 = 1;
|
||||
break;
|
||||
|
||||
case -BGC_DIRECTION_X2:
|
||||
bgc_vector3_reverse_fp64(axis, &basis->e2);
|
||||
state->e2 = 1;
|
||||
break;
|
||||
|
||||
case BGC_DIRECTION_X3:
|
||||
bgc_vector3_copy_fp64(axis, &basis->e3);
|
||||
state->e3 = 1;
|
||||
break;
|
||||
|
||||
case -BGC_DIRECTION_X3:
|
||||
bgc_vector3_reverse_fp64(axis, &basis->e3);
|
||||
state->e3 = 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
int bgc_basis3_make_from_directions_fp64(
|
||||
const int primary_direction, const BgcVector3FP64* primary_vector,
|
||||
const int auxiliary_direction, const BgcVector3FP64* auxiliary_vector,
|
||||
BgcBasis3FP64* basis
|
||||
) {
|
||||
const int direstion_validation_code = _bgc_basis3_validate_directions(primary_direction, auxiliary_direction);
|
||||
|
||||
if (direstion_validation_code != BGC_SUCCESS) {
|
||||
return direstion_validation_code;
|
||||
}
|
||||
|
||||
const double primary_square_modulus = bgc_vector3_get_square_modulus_fp64(primary_vector);
|
||||
|
||||
if (primary_square_modulus <= BGC_SQUARE_EPSYLON_FP64) {
|
||||
return BGC_ERROR_BASIS3_PRIMARY_VECTOR_IS_ZERO;
|
||||
}
|
||||
|
||||
const double auxiliary_square_modulus = bgc_vector3_get_square_modulus_fp64(auxiliary_vector);
|
||||
|
||||
if (auxiliary_square_modulus <= BGC_SQUARE_EPSYLON_FP64) {
|
||||
return BGC_ERROR_BASIS3_AUXILIARY_VECTOR_IS_ZERO;
|
||||
}
|
||||
|
||||
BgcVector3FP64 orthogonal;
|
||||
|
||||
bgc_vector3_get_cross_product_fp64(primary_vector, auxiliary_vector, &orthogonal);
|
||||
|
||||
const double orthogonal_square_modulus = bgc_vector3_get_square_modulus_fp64(&orthogonal);
|
||||
|
||||
if (orthogonal_square_modulus <= BGC_SQUARE_EPSYLON_FP64 * primary_square_modulus * auxiliary_square_modulus) {
|
||||
return BGC_ERROR_BASIS3_PRIMARY_AUXILIARY_PARALLEL;
|
||||
}
|
||||
|
||||
BgcVector3FP64 primary_axis, auxiliary_axis;
|
||||
|
||||
bgc_vector3_multiply_fp64(primary_vector, sqrt(1.0 / primary_square_modulus), &primary_axis);
|
||||
|
||||
bgc_vector3_subtract_scaled_fp64(auxiliary_vector, &primary_axis, bgc_vector3_get_scalar_product_fp64(auxiliary_vector, &primary_axis), &auxiliary_axis);
|
||||
|
||||
bgc_vector3_multiply_fp64(&auxiliary_axis, sqrt(1.0 / bgc_vector3_get_square_modulus_fp64(&auxiliary_axis)), &auxiliary_axis);
|
||||
|
||||
_BgcBasis3State state;
|
||||
|
||||
_bgc_basis3_state_reset(&state);
|
||||
|
||||
_bgc_basis3_load_axis_fp64(primary_direction, &primary_axis, &state, basis);
|
||||
_bgc_basis3_load_axis_fp64(auxiliary_direction, &auxiliary_axis, &state, basis);
|
||||
|
||||
if (!state.e1) {
|
||||
bgc_vector3_get_cross_product_fp64(&basis->e2, &basis->e3, &basis->e1);
|
||||
bgc_vector3_normalize_fp64(&basis->e1);
|
||||
return BGC_SUCCESS;
|
||||
}
|
||||
|
||||
if (!state.e2) {
|
||||
bgc_vector3_get_cross_product_fp64(&basis->e3, &basis->e1, &basis->e2);
|
||||
bgc_vector3_normalize_fp64(&basis->e2);
|
||||
return BGC_SUCCESS;
|
||||
}
|
||||
|
||||
bgc_vector3_get_cross_product_fp64(&basis->e1, &basis->e2, &basis->e3);
|
||||
bgc_vector3_normalize_fp64(&basis->e3);
|
||||
|
||||
return BGC_SUCCESS;
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue