Butis-C/butis/motor.h

#ifndef _BUTIS_MOTOR_H_INCLUDE_
#define _BUTIS_MOTOR_H_INCLUDE_

#include <basic-geometry.h>

// =================== Types ==================== //

typedef struct {
    BgcVector3FP32 vector, momentum;
} BtMotorFP32;

typedef struct {
    BgcVector3FP64 vector, momentum;
} BtMotorFP64;

// =================== Reset ==================== //

inline void bt_motor_reset_fp32(BtMotorFP32* motor)
{
    bgc_vector3_reset_fp32(&motor->vector);
    bgc_vector3_reset_fp32(&motor->momentum);
}

inline void bt_motor_reset_fp64(BtMotorFP64* motor)
{
    bgc_vector3_reset_fp64(&motor->vector);
    bgc_vector3_reset_fp64(&motor->momentum);
}

// ==================== Copy ==================== //

inline void bt_motor_copy_fp32(const BtMotorFP32* source, BtMotorFP32* destination)
{
    bgc_vector3_copy_fp32(&source->vector, &destination->vector);
    bgc_vector3_copy_fp32(&source->momentum, &destination->momentum);
}

inline void bt_motor_copy_fp64(const BtMotorFP64* source, BtMotorFP64* destination)
{
    bgc_vector3_copy_fp64(&source->vector, &destination->vector);
    bgc_vector3_copy_fp64(&source->momentum, &destination->momentum);
}

// ================== Convert =================== //

inline void bt_motor_convert_fp64_to_fp32(const BtMotorFP64* source, BtMotorFP32* destination)
{
    bgc_vector3_convert_fp64_to_fp32(&source->vector, &destination->vector);
    bgc_vector3_convert_fp64_to_fp32(&source->momentum, &destination->momentum);
}

inline void bt_motor_convert_fp32_to_fp64(const BtMotorFP32* source, BtMotorFP64* destination)
{
    bgc_vector3_convert_fp32_to_fp64(&source->vector, &destination->vector);
    bgc_vector3_convert_fp32_to_fp64(&source->momentum, &destination->momentum);
}

// ================== Is Zero =================== //

inline int bt_motor_is_zero_fp32(const BtMotorFP32* motor)
{
    return bgc_vector3_is_zero_fp32(&motor->vector) && bgc_vector3_is_zero_fp32(&motor->momentum);
}

inline int bt_motor_is_zero_fp64(const BtMotorFP64* motor)
{
    return bgc_vector3_is_zero_fp64(&motor->vector) && bgc_vector3_is_zero_fp64(&motor->momentum);
}

// ================= Is Vector ================== //

inline int bt_motor_is_vector_fp32(const BtMotorFP32* motor)
{
    return bgc_vector3_are_orthogonal_fp32(&motor->vector, &motor->momentum);
}

inline int bt_motor_is_vector_fp64(const BtMotorFP64* motor)
{
    return bgc_vector3_are_orthogonal_fp64(&motor->vector, &motor->momentum);
}

// ============== Is Pure Momentum ============== //

inline int bt_motor_is_momentum_fp32(const BtMotorFP32* motor)
{
    return bgc_vector3_is_zero_fp32(&motor->vector);
}

inline int bt_motor_is_momentum_fp64(const BtMotorFP64* motor)
{
    return bgc_vector3_is_zero_fp64(&motor->vector);
}

// ==================== Set ===================== //

inline void bt_motor_set_fp32(const BgcVector3FP32* vector, const BgcVector3FP32* momentum, BtMotorFP32* motor)
{
    bgc_vector3_copy_fp32(vector, &motor->vector);
    bgc_vector3_copy_fp32(momentum, &motor->momentum);
}

inline void bt_motor_set_fp64(const BgcVector3FP64* vector, const BgcVector3FP64* momentum, BtMotorFP64* motor)
{
    bgc_vector3_copy_fp64(vector, &motor->vector);
    bgc_vector3_copy_fp64(momentum, &motor->momentum);
}

// ================= Set Vector ================= //

inline void bt_motor_set_vector_fp32(const BgcVector3FP32* vector, BtMotorFP32* motor)
{
    bgc_vector3_copy_fp32(vector, &motor->vector);
}

inline void bt_motor_set_vector_fp64(const BgcVector3FP64* vector, BtMotorFP64* motor)
{
    bgc_vector3_copy_fp64(vector, &motor->vector);
}

// ================ Set Momentum ================ //

inline void bt_motor_set_momentum_fp32(const BgcVector3FP32* momentum, BtMotorFP32* motor)
{
    bgc_vector3_copy_fp32(momentum, &motor->momentum);
}

inline void bt_motor_set_momentum_fp64(const BgcVector3FP64* momentum, BtMotorFP64* motor)
{
    bgc_vector3_copy_fp64(momentum, &motor->momentum);
}

// ================= Set Values ================= //

inline void bt_motor_set_values_fp32(const float vector_x1, const float vector_x2, const float vector_x3, const float momentum_x1, const float momentum_x2, const float momentum_x3, BtMotorFP32* motor)
{
    bgc_vector3_set_values_fp32(vector_x1, vector_x2, vector_x3, &motor->vector);
    bgc_vector3_set_values_fp32(momentum_x1, momentum_x2, momentum_x3, &motor->momentum);
}

inline void bt_motor_set_values_fp64(const double vector_x1, const double vector_x2, const double vector_x3, const double momentum_x1, const double momentum_x2, const double momentum_x3, BtMotorFP64* motor)
{
    bgc_vector3_set_values_fp64(vector_x1, vector_x2, vector_x3, &motor->vector);
    bgc_vector3_set_values_fp64(momentum_x1, momentum_x2, momentum_x3, &motor->momentum);
}

// ==================== Make ==================== //

inline void bt_motor_make_fp32(const BgcVector3FP32* point, const BgcVector3FP32* vector, const BgcVector3FP32* momentum, BtMotorFP32* motor)
{
    bgc_vector3_copy_fp32(vector, &motor->vector);
    bgc_vector3_get_cross_product_fp32(point, vector, &motor->momentum);
    bgc_vector3_add_fp32(&motor->momentum, momentum, &motor->momentum);
}

inline void bt_motor_make_fp64(const BgcVector3FP64* point, const BgcVector3FP64* vector, const BgcVector3FP64* momentum, BtMotorFP64* motor)
{
    bgc_vector3_copy_fp64(vector, &motor->vector);
    bgc_vector3_get_cross_product_fp64(point, vector, &motor->momentum);
    bgc_vector3_add_fp64(&motor->momentum, momentum, &motor->momentum);
}

// ============== Make with Values ============== //

inline void bt_motor_make_with_values_fp32(
    const float point_x1, const float point_x2, const float point_x3,
    const float vector_x1, const float vector_x2, const float vector_x3,
    const float momentum_x1, const float momentum_x2, const float momentum_x3,
    BtMotorFP32* motor)
{
    motor->vector.x1 = vector_x1;
    motor->vector.x2 = vector_x2;
    motor->vector.x3 = vector_x3;

    motor->momentum.x1 = (point_x2 * vector_x3 - point_x3 * vector_x2) + momentum_x1;
    motor->momentum.x2 = (point_x3 * vector_x1 - point_x1 * vector_x3) + momentum_x2;
    motor->momentum.x3 = (point_x1 * vector_x2 - point_x2 * vector_x1) + momentum_x3;
}

inline void bt_motor_make_with_values_fp64(
    const double point_x1, const double point_x2, const double point_x3,
    const double vector_x1, const double vector_x2, const double vector_x3,
    const double momentum_x1, const double momentum_x2, const double momentum_x3,
    BtMotorFP64* motor)
{
    motor->vector.x1 = vector_x1;
    motor->vector.x2 = vector_x2;
    motor->vector.x3 = vector_x3;

    motor->momentum.x1 = (point_x2 * vector_x3 - point_x3 * vector_x2) + momentum_x1;
    motor->momentum.x2 = (point_x3 * vector_x1 - point_x1 * vector_x3) + momentum_x2;
    motor->momentum.x3 = (point_x1 * vector_x2 - point_x2 * vector_x1) + momentum_x3;
}

// ============== Make pure Vector ============== //

inline void bt_motor_make_pure_vector_fp32(const BgcVector3FP32* point, const BgcVector3FP32* vector, BtMotorFP32* motor)
{
    bgc_vector3_copy_fp32(vector, &motor->vector);
    bgc_vector3_get_cross_product_fp32(point, vector, &motor->momentum);
}

inline void bt_motor_make_pure_vector_fp64(const BgcVector3FP64* point, const BgcVector3FP64* vector, BtMotorFP64* motor)
{
    bgc_vector3_copy_fp64(vector, &motor->vector);
    bgc_vector3_get_cross_product_fp64(point, vector, &motor->momentum);
}

// ============= Make pure Momentum ============= //

inline void bt_motor_make_pure_momentum_fp32(const BgcVector3FP32* momentum, BtMotorFP32* motor)
{
    bgc_vector3_reset_fp32(&motor->vector);
    bgc_vector3_copy_fp32(momentum, &motor->momentum);
}

inline void bt_motor_make_pure_momentum_fp64(const BgcVector3FP64* momentum, BtMotorFP64* motor)
{
    bgc_vector3_reset_fp64(&motor->vector);
    bgc_vector3_copy_fp64(momentum, &motor->momentum);
}

// ==================== Swap ==================== //

inline void bt_motor_swap_fp32(BtMotorFP32* motor1, BtMotorFP32* motor2)
{
    bgc_vector3_swap_fp32(&motor2->vector, &motor2->vector);
    bgc_vector3_swap_fp32(&motor2->momentum, &motor2->momentum);
}

inline void bt_motor_swap_fp64(BtMotorFP64* motor1, BtMotorFP64* motor2)
{
    bgc_vector3_swap_fp64(&motor2->vector, &motor2->vector);
    bgc_vector3_swap_fp64(&motor2->momentum, &motor2->momentum);
}

// =================== Shift ==================== //

inline void bt_motor_shift_fp32(const BgcVector3FP32* shift, BtMotorFP32* motor)
{
    BgcVector3FP32 momentum_change;

    bgc_vector3_get_cross_product_fp32(&motor->vector, shift, &momentum_change);

    bgc_vector3_add_fp32(&motor->momentum, &momentum_change, &motor->momentum);
}

inline void bt_motor_shift_fp64(const BgcVector3FP64* shift, BtMotorFP64* motor)
{
    BgcVector3FP64 momentum_change;

    bgc_vector3_get_cross_product_fp64(&motor->vector, shift, &momentum_change);

    bgc_vector3_add_fp64(&motor->momentum, &momentum_change, &motor->momentum);
}

inline void bt_motor_get_shifted_fp32(const BtMotorFP32* motor, const BgcVector3FP32* shift, BtMotorFP32* shifted)
{
    BgcVector3FP32 momentum_change;

    bgc_vector3_get_cross_product_fp32(&motor->vector, shift, &momentum_change);

    bgc_vector3_copy_fp32(&motor->vector, &shifted->vector);
    bgc_vector3_add_fp32(&motor->momentum, &momentum_change, &shifted->momentum);
}

inline void bt_motor_get_shifted_fp64(const BtMotorFP64* motor, const BgcVector3FP64* shift, BtMotorFP64* shifted)
{
    BgcVector3FP64 momentum_change;

    bgc_vector3_get_cross_product_fp64(&motor->vector, shift, &momentum_change);

    bgc_vector3_copy_fp64(&motor->vector, &shifted->vector);
    bgc_vector3_add_fp64(&motor->momentum, &momentum_change, &shifted->momentum);
}

// ==================== Add ===================== //

inline void bt_motor_add_fp32(const BtMotorFP32* motor1, const BtMotorFP32* motor2, BtMotorFP32* sum)
{
    bgc_vector3_add_fp32(&motor1->vector, &motor2->vector, &sum->vector);
    bgc_vector3_add_fp32(&motor1->momentum, &motor2->momentum, &sum->momentum);
}

inline void bt_motor_add_fp64(const BtMotorFP64* motor1, const BtMotorFP64* motor2, BtMotorFP64* sum)
{
    bgc_vector3_add_fp64(&motor1->vector, &motor2->vector, &sum->vector);
    bgc_vector3_add_fp64(&motor1->momentum, &motor2->momentum, &sum->momentum);
}

// ================= Add Scaled ================= //

inline void bt_motor_add_scaled_fp32(const BtMotorFP32* basic, const BtMotorFP32* scalable, const float scale, BtMotorFP32* sum)
{
    bgc_vector3_add_scaled_fp32(&basic->vector, &scalable->vector, scale, &sum->vector);
    bgc_vector3_add_scaled_fp32(&basic->momentum, &scalable->momentum, scale, &sum->momentum);
}

inline void bt_motor_add_scaled_fp64(const BtMotorFP64* basic, const BtMotorFP64* scalable, const double scale, BtMotorFP64* sum)
{
    bgc_vector3_add_scaled_fp64(&basic->vector, &scalable->vector, scale, &sum->vector);
    bgc_vector3_add_scaled_fp64(&basic->momentum, &scalable->momentum, scale, &sum->momentum);
}

// ================= Add Vector ================= //

inline void bt_motor_add_vector_fp32(const BtMotorFP32* basic, const BgcVector3FP32* point, const BgcVector3FP32* vector, BtMotorFP32* sum)
{
    BgcVector3FP32 momentum_change;

    bgc_vector3_get_cross_product_fp32(point, vector, &momentum_change);

    bgc_vector3_add_fp32(&basic->vector, vector, &sum->vector);
    bgc_vector3_add_fp32(&basic->momentum, &momentum_change, &sum->momentum);
}

inline void bt_motor_add_vector_fp64(const BtMotorFP64* basic, const BgcVector3FP64* point, const BgcVector3FP64* vector, BtMotorFP64* sum)
{
    BgcVector3FP64 momentum_change;

    bgc_vector3_get_cross_product_fp64(point, vector, &momentum_change);

    bgc_vector3_add_fp64(&basic->vector, vector, &sum->vector);
    bgc_vector3_add_fp64(&basic->momentum, &momentum_change, &sum->momentum);
}

// ============= Add Scaled Vector ============== //

inline void bt_motor_add_scaled_vector_fp32(const BtMotorFP32* basic, const BgcVector3FP32* point, const BgcVector3FP32* vector, const float scale, BtMotorFP32* sum)
{
    BgcVector3FP32 momentum_change;

    bgc_vector3_get_cross_product_fp32(point, vector, &momentum_change);

    bgc_vector3_add_scaled_fp32(&basic->vector, vector, scale, &sum->vector);
    bgc_vector3_add_scaled_fp32(&basic->momentum, &momentum_change, scale, &sum->momentum);
}

inline void bt_motor_add_scaled_vector_fp64(const BtMotorFP64* basic, const BgcVector3FP64* point, const BgcVector3FP64* vector, const double scale, BtMotorFP64* sum)
{
    BgcVector3FP64 momentum_change;

    bgc_vector3_get_cross_product_fp64(point, vector, &momentum_change);

    bgc_vector3_add_scaled_fp64(&basic->vector, vector, scale, &sum->vector);
    bgc_vector3_add_scaled_fp64(&basic->momentum, &momentum_change, scale, &sum->momentum);
}

// ================ Add Momentum ================ //

inline void bt_motor_add_momentum_fp32(const BtMotorFP32* basic, const BgcVector3FP32* momentum, BtMotorFP32* sum)
{
    bgc_vector3_copy_fp32(&basic->vector, &sum->vector);
    bgc_vector3_add_fp32(&basic->momentum, momentum, &sum->momentum);
}

inline void bt_motor_add_momentum_fp64(const BtMotorFP64* basic, const BgcVector3FP64* momentum, BtMotorFP64* sum)
{
    bgc_vector3_copy_fp64(&basic->vector, &sum->vector);
    bgc_vector3_add_fp64(&basic->momentum, momentum, &sum->momentum);
}

// ============ Add Scaled Momentum ============= //

inline void bt_motor_add_scaled_momentum_fp32(const BtMotorFP32* basic, const BgcVector3FP32* momentum, const float scale, BtMotorFP32* sum)
{
    bgc_vector3_copy_fp32(&basic->vector, &sum->vector);
    bgc_vector3_add_scaled_fp32(&basic->momentum, momentum, scale, &sum->momentum);
}

inline void bt_motor_add_scaled_momentum_fp64(const BtMotorFP64* basic, const BgcVector3FP64* momentum, const double scale, BtMotorFP64* sum)
{
    bgc_vector3_copy_fp64(&basic->vector, &sum->vector);
    bgc_vector3_add_scaled_fp64(&basic->momentum, momentum, scale, &sum->momentum);
}

// ================== Subtract ================== //

inline void bt_motor_subtract_fp32(const BtMotorFP32* minuend, const BtMotorFP32* subtrahend, BtMotorFP32* difference)
{
    bgc_vector3_subtract_fp32(&minuend->vector, &subtrahend->vector, &difference->vector);
    bgc_vector3_subtract_fp32(&minuend->momentum, &subtrahend->momentum, &difference->momentum);
}

inline void bt_motor_subtract_fp64(const BtMotorFP64* minuend, const BtMotorFP64* subtrahend, BtMotorFP64* difference)
{
    bgc_vector3_subtract_fp64(&minuend->vector, &subtrahend->vector, &difference->vector);
    bgc_vector3_subtract_fp64(&minuend->momentum, &subtrahend->momentum, &difference->momentum);
}

// ============== Subtract Vector =============== //

inline void bt_motor_subtract_vector_fp32(const BtMotorFP32* minuend, const BgcVector3FP32* point, const BgcVector3FP32* vector, BtMotorFP32* difference)
{
    BgcVector3FP32 momentum_change;

    bgc_vector3_get_cross_product_fp32(point, vector, &momentum_change);

    bgc_vector3_subtract_fp32(&minuend->vector, vector, &difference->vector);
    bgc_vector3_subtract_fp32(&minuend->momentum, &momentum_change, &difference->momentum);
}

inline void bt_motor_subtract_vector_fp64(const BtMotorFP64* minuend, const BgcVector3FP64* point, const BgcVector3FP64* vector, BtMotorFP64* difference)
{
    BgcVector3FP64 momentum_change;

    bgc_vector3_get_cross_product_fp64(point, vector, &momentum_change);

    bgc_vector3_subtract_fp64(&minuend->vector, vector, &difference->vector);
    bgc_vector3_subtract_fp64(&minuend->momentum, &momentum_change, &difference->momentum);
}

// ============= Subtract Momentum ============== //

inline void bt_motor_subtract_momentum_fp32(const BtMotorFP32* minuend, const BgcVector3FP32* momentum, BtMotorFP32* difference)
{
    bgc_vector3_copy_fp32(&minuend->vector, &difference->vector);
    bgc_vector3_subtract_fp32(&minuend->momentum, momentum, &difference->momentum);
}

inline void bt_motor_subtract_momentum_fp64(const BtMotorFP64* minuend, const BgcVector3FP64* momentum, BtMotorFP64* difference)
{
    bgc_vector3_copy_fp64(&minuend->vector, &difference->vector);
    bgc_vector3_subtract_fp64(&minuend->momentum, momentum, &difference->momentum);
}

// ================== Multiply ================== //

inline void bt_motor_multiply_fp32(const BtMotorFP32* multiplicand, const float multiplier, BtMotorFP32* product)
{
    bgc_vector3_multiply_fp32(&multiplicand->vector, multiplier, &product->vector);
    bgc_vector3_multiply_fp32(&multiplicand->momentum, multiplier, &product->momentum);
}

inline void bt_motor_multiply_fp64(const BtMotorFP64* multiplicand, const double multiplier, BtMotorFP64* product)
{
    bgc_vector3_multiply_fp64(&multiplicand->vector, multiplier, &product->vector);
    bgc_vector3_multiply_fp64(&multiplicand->momentum, multiplier, &product->momentum);
}

// =================== Divide =================== //

inline void bt_motor_divide_fp32(const BtMotorFP32* dividend, const float divisor, BtMotorFP32* quotient)
{
    bt_motor_multiply_fp32(dividend, 1.0f / divisor, quotient);
}

inline void bt_motor_divide_fp64(const BtMotorFP64* dividend, const double divisor, BtMotorFP64* quotient)
{
    bt_motor_multiply_fp64(dividend, 1.0 / divisor, quotient);
}

// ============= Get Central Point ============== //

inline int bt_motor_get_central_point_fp32(const BtMotorFP32* motor, BgcVector3FP32* point)
{
    const float square_modulus = bgc_vector3_get_square_modulus_fp32(&motor->vector);

    if (square_modulus <= BGC_SQUARE_EPSYLON_FP32) {
        return 0;
    }

    bgc_vector3_get_cross_product_fp32(&motor->vector, &motor->momentum, point);
    bgc_vector3_divide_fp32(point, square_modulus, point);

    return 1;
}

inline int bt_motor_get_central_point_fp64(const BtMotorFP64* motor, BgcVector3FP64* point)
{
    const double square_modulus = bgc_vector3_get_square_modulus_fp64(&motor->vector);

    if (square_modulus <= BGC_SQUARE_EPSYLON_FP64) {
        return 0;
    }

    bgc_vector3_get_cross_product_fp64(&motor->vector, &motor->momentum, point);
    bgc_vector3_divide_fp64(point, square_modulus, point);

    return 1;
}

#endif