bgc-c/basic-geometry/basis3.c

#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;
}