416 lines
16 KiB
C
416 lines
16 KiB
C
#ifndef _BUTIS_MOTOR_H_INCLUDE_
|
|
#define _BUTIS_MOTOR_H_INCLUDE_
|
|
|
|
#include <basic-geometry.h>
|
|
|
|
// =================== Types ==================== //
|
|
|
|
typedef struct {
|
|
BgcVector3FP32 point, vector, momentum;
|
|
} BtMotorFP32;
|
|
|
|
typedef struct {
|
|
BgcVector3FP64 point, vector, momentum;
|
|
} BtMotorFP64;
|
|
|
|
// =================== Reset ==================== //
|
|
|
|
inline void bt_motor_reset_fp32(BtMotorFP32* motor)
|
|
{
|
|
bgc_vector3_reset_fp32(&motor->point);
|
|
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->point);
|
|
bgc_vector3_reset_fp64(&motor->vector);
|
|
bgc_vector3_reset_fp64(&motor->momentum);
|
|
}
|
|
|
|
// =============== Reset At Point =============== //
|
|
|
|
inline void bt_motor_reset_at_point_fp32(const BgcVector3FP32* point, BtMotorFP32* motor)
|
|
{
|
|
bgc_vector3_copy_fp32(point, &motor->point);
|
|
bgc_vector3_reset_fp32(&motor->vector);
|
|
bgc_vector3_reset_fp32(&motor->momentum);
|
|
}
|
|
|
|
inline void bt_motor_reset_at_point_fp64(const BgcVector3FP64* point, BtMotorFP64* motor)
|
|
{
|
|
bgc_vector3_copy_fp64(point, &motor->point);
|
|
bgc_vector3_reset_fp64(&motor->vector);
|
|
bgc_vector3_reset_fp64(&motor->momentum);
|
|
}
|
|
|
|
// ============== Reset At Values =============== //
|
|
|
|
inline void bt_motor_reset_with_values_fp32(const float x1, const float x2, const float x3, BtMotorFP32* motor)
|
|
{
|
|
bgc_vector3_set_values_fp32(x1, x2, x3, &motor->point);
|
|
bgc_vector3_reset_fp32(&motor->vector);
|
|
bgc_vector3_reset_fp32(&motor->momentum);
|
|
}
|
|
|
|
inline void bt_motor_reset_with_values_fp64(const double x1, const double x2, const double x3, BtMotorFP64* motor)
|
|
{
|
|
bgc_vector3_set_values_fp64(x1, x2, x3, &motor->point);
|
|
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->point, &destination->point);
|
|
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->point, &destination->point);
|
|
bgc_vector3_copy_fp64(&source->vector, &destination->vector);
|
|
bgc_vector3_copy_fp64(&source->momentum, &destination->momentum);
|
|
}
|
|
|
|
// ================== Convert =================== //
|
|
|
|
inline void bt_motor_convert_fp32_to_fp64(const BtMotorFP32* source, BtMotorFP64* destination)
|
|
{
|
|
bgc_vector3_convert_fp32_to_fp64(&source->point, &destination->point);
|
|
bgc_vector3_convert_fp32_to_fp64(&source->vector, &destination->vector);
|
|
bgc_vector3_convert_fp32_to_fp64(&source->momentum, &destination->momentum);
|
|
}
|
|
|
|
inline void bt_motor_convert_fp64_to_fp32(const BtMotorFP64* source, BtMotorFP32* destination)
|
|
{
|
|
bgc_vector3_convert_fp64_to_fp32(&source->point, &destination->point);
|
|
bgc_vector3_convert_fp64_to_fp32(&source->vector, &destination->vector);
|
|
bgc_vector3_convert_fp64_to_fp32(&source->momentum, &destination->momentum);
|
|
}
|
|
|
|
// ==================== Swap ==================== //
|
|
|
|
inline void bt_motor_swap_fp32(BtMotorFP32* motor1, BtMotorFP32* motor2)
|
|
{
|
|
bgc_vector3_swap_fp32(&motor2->point, &motor2->point);
|
|
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->point, &motor2->point);
|
|
bgc_vector3_swap_fp64(&motor2->vector, &motor2->vector);
|
|
bgc_vector3_swap_fp64(&motor2->momentum, &motor2->momentum);
|
|
}
|
|
|
|
// ================== Shift At ================== //
|
|
|
|
inline void bt_motor_shift_at_fp32(const BtMotorFP32* motor, const BgcVector3FP32* shift, BtMotorFP32* moved)
|
|
{
|
|
BgcVector3FP32 momentum_difference;
|
|
bgc_vector3_get_cross_product_fp32(shift, &motor->vector, &momentum_difference);
|
|
bgc_vector3_copy_fp32(&motor->vector, &moved->vector);
|
|
bgc_vector3_subtract_fp32(&motor->momentum, &momentum_difference, &moved->vector);
|
|
bgc_vector3_add_fp32(&motor->point, shift, &moved->point);
|
|
}
|
|
|
|
inline void bt_motor_shift_at_fp64(const BtMotorFP64* motor, const BgcVector3FP64* shift, BtMotorFP64* moved)
|
|
{
|
|
BgcVector3FP64 momentum_difference;
|
|
bgc_vector3_get_cross_product_fp64(shift, &motor->vector, &momentum_difference);
|
|
bgc_vector3_subtract_fp64(&motor->momentum, &momentum_difference, &moved->vector);
|
|
bgc_vector3_copy_fp64(&motor->vector, &moved->vector);
|
|
bgc_vector3_add_fp64(&motor->point, shift, &moved->point);
|
|
}
|
|
|
|
// ================== Shift To ================== //
|
|
|
|
inline void bt_motor_shift_to_fp32(const BtMotorFP32* motor, const BgcVector3FP32* destination, BtMotorFP32* moved)
|
|
{
|
|
BgcVector3FP32 shift;
|
|
BgcVector3FP32 momentum_change;
|
|
bgc_vector3_subtract_fp32(&motor->point, destination, &shift);
|
|
bgc_vector3_get_cross_product_fp32(&shift, &motor->vector, &momentum_change);
|
|
bgc_vector3_copy_fp32(destination, &moved->point);
|
|
bgc_vector3_copy_fp32(&motor->vector, &moved->vector);
|
|
bgc_vector3_add_fp32(&motor->momentum, &momentum_change, &moved->momentum);
|
|
}
|
|
|
|
inline void bt_motor_shift_to_fp64(const BtMotorFP64* motor, const BgcVector3FP64* destination, BtMotorFP64* moved)
|
|
{
|
|
BgcVector3FP64 shift;
|
|
BgcVector3FP64 momentum_change;
|
|
bgc_vector3_subtract_fp64(&motor->point, destination, &shift);
|
|
bgc_vector3_get_cross_product_fp64(&shift, &motor->vector, &momentum_change);
|
|
bgc_vector3_copy_fp64(destination, &moved->point);
|
|
bgc_vector3_copy_fp64(&motor->vector, &moved->vector);
|
|
bgc_vector3_add_fp64(&motor->momentum, &momentum_change, &moved->momentum);
|
|
}
|
|
|
|
// ================== Multiply ================== //
|
|
|
|
inline void bt_motor_multiply_at_number_fp32(const BtMotorFP32* multiplicand, const float multiplier, BtMotorFP32* product)
|
|
{
|
|
bgc_vector3_copy_fp32(&multiplicand->point, &product->point);
|
|
bgc_vector3_multiply_fp32(&multiplicand->vector, multiplier, &product->vector);
|
|
bgc_vector3_multiply_fp32(&multiplicand->momentum, multiplier, &product->momentum);
|
|
}
|
|
|
|
inline void bt_motor_multiply_at_number_fp64(const BtMotorFP64* multiplicand, const double multiplier, BtMotorFP64* product)
|
|
{
|
|
bgc_vector3_copy_fp64(&multiplicand->point, &product->point);
|
|
bgc_vector3_multiply_fp64(&multiplicand->vector, multiplier, &product->vector);
|
|
bgc_vector3_multiply_fp64(&multiplicand->momentum, multiplier, &product->momentum);
|
|
}
|
|
|
|
// =================== Divide =================== //
|
|
|
|
inline void bt_motor_divide_at_number_fp32(const BtMotorFP32* divident, const float divisor, BtMotorFP32* quotient)
|
|
{
|
|
bt_motor_multiply_at_number_fp32(divident, 1.0f / divisor, quotient);
|
|
}
|
|
|
|
inline void bt_motor_divide_at_number_fp64(const BtMotorFP64* divident, const double divisor, BtMotorFP64* quotient)
|
|
{
|
|
bt_motor_multiply_at_number_fp64(divident, 1.0 / divisor, quotient);
|
|
}
|
|
|
|
// ==================== Add ===================== //
|
|
|
|
inline void bt_motor_add_vector_fp32(const BgcVector3FP32* point, const BgcVector3FP32* vector, BtMotorFP32* sum)
|
|
{
|
|
BgcVector3FP32 distance, momentum;
|
|
|
|
bgc_vector3_subtract_fp32(point, &sum->point, &distance);
|
|
bgc_vector3_get_cross_product_fp32(&distance, vector, &momentum);
|
|
|
|
bgc_vector3_add_fp32(&sum->vector, vector, &sum->vector);
|
|
bgc_vector3_add_fp32(&sum->momentum, &momentum, &sum->momentum);
|
|
}
|
|
|
|
inline void bt_motor_add_vector_fp64(const BgcVector3FP64* point, const BgcVector3FP64* vector, BtMotorFP64* sum)
|
|
{
|
|
BgcVector3FP64 distance, momentum;
|
|
|
|
bgc_vector3_subtract_fp64(point, &sum->point, &distance);
|
|
bgc_vector3_get_cross_product_fp64(&distance, vector, &momentum);
|
|
|
|
bgc_vector3_add_fp64(&sum->vector, vector, &sum->vector);
|
|
bgc_vector3_add_fp64(&sum->momentum, &momentum, &sum->momentum);
|
|
}
|
|
|
|
// ================= Add Motor ================== //
|
|
|
|
inline void bt_motor_add_motor_fp32(const BtMotorFP32* motor, BtMotorFP32* sum)
|
|
{
|
|
BgcVector3FP32 distance, momentum;
|
|
|
|
bgc_vector3_subtract_fp32(&motor->point, &sum->point, &distance);
|
|
bgc_vector3_get_cross_product_fp32(&distance, &motor->vector, &momentum);
|
|
|
|
bgc_vector3_add_fp32(&momentum, &motor->vector, &momentum);
|
|
|
|
bgc_vector3_add_fp32(&sum->vector, &motor->vector, &sum->vector);
|
|
bgc_vector3_add_fp32(&sum->momentum, &momentum, &sum->momentum);
|
|
}
|
|
|
|
inline void bt_motor_add_motor_fp64(const BtMotorFP64* motor, BtMotorFP64* sum)
|
|
{
|
|
BgcVector3FP64 distance, momentum;
|
|
|
|
bgc_vector3_subtract_fp64(&motor->point, &sum->point, &distance);
|
|
bgc_vector3_get_cross_product_fp64(&distance, &motor->vector, &momentum);
|
|
|
|
bgc_vector3_add_fp64(&momentum, &motor->vector, &momentum);
|
|
|
|
bgc_vector3_add_fp64(&sum->vector, &motor->vector, &sum->vector);
|
|
bgc_vector3_add_fp64(&sum->momentum, &momentum, &sum->momentum);
|
|
}
|
|
|
|
// ================= Add Scaled ================= //
|
|
|
|
inline void bt_motor_add_scaled_vector_fp32(const BgcVector3FP32* point, const BgcVector3FP32* vector, const float scale, BtMotorFP32* sum)
|
|
{
|
|
BgcVector3FP32 distance, momentum;
|
|
|
|
bgc_vector3_subtract_fp32(point, &sum->point, &distance);
|
|
bgc_vector3_get_cross_product_fp32(&distance, vector, &momentum);
|
|
|
|
bgc_vector3_add_scaled_fp32(&sum->vector, vector, scale, &sum->vector);
|
|
bgc_vector3_add_scaled_fp32(&sum->momentum, &momentum, scale, &sum->momentum);
|
|
}
|
|
|
|
inline void bt_motor_add_scaled_vector_fp64(const BgcVector3FP64* point, const BgcVector3FP64* vector, const double scale, BtMotorFP64* sum)
|
|
{
|
|
BgcVector3FP64 distance, momentum;
|
|
|
|
bgc_vector3_subtract_fp64(point, &sum->point, &distance);
|
|
bgc_vector3_get_cross_product_fp64(&distance, vector, &momentum);
|
|
|
|
bgc_vector3_add_scaled_fp64(&sum->vector, vector, scale, &sum->vector);
|
|
bgc_vector3_add_scaled_fp64(&sum->momentum, &momentum, scale, &sum->momentum);
|
|
}
|
|
|
|
// ================== Subtract ================== //
|
|
|
|
inline void bt_motor_subtract_vector_fp32(const BgcVector3FP32* point, const BgcVector3FP32* vector, BtMotorFP32* difference)
|
|
{
|
|
BgcVector3FP32 distance, momentum;
|
|
|
|
bgc_vector3_subtract_fp32(point, &difference->point, &distance);
|
|
bgc_vector3_get_cross_product_fp32(&distance, vector, &momentum);
|
|
|
|
bgc_vector3_subtract_fp32(&difference->vector, vector, &difference->vector);
|
|
bgc_vector3_subtract_fp32(&difference->momentum, &momentum, &difference->momentum);
|
|
}
|
|
|
|
inline void bt_motor_subtract_vector_fp64(const BgcVector3FP64* point, const BgcVector3FP64* vector, BtMotorFP64* difference)
|
|
{
|
|
BgcVector3FP64 distance, momentum;
|
|
|
|
bgc_vector3_subtract_fp64(point, &difference->point, &distance);
|
|
bgc_vector3_get_cross_product_fp64(&distance, vector, &momentum);
|
|
|
|
bgc_vector3_subtract_fp64(&difference->vector, vector, &difference->vector);
|
|
bgc_vector3_subtract_fp64(&difference->momentum, &momentum, &difference->momentum);
|
|
}
|
|
|
|
// ============== Subtract Scaled =============== //
|
|
|
|
inline void bt_motor_subtract_scaled_vector_fp32(const BgcVector3FP32* point, const BgcVector3FP32* vector, const float scale, BtMotorFP32* motor)
|
|
{
|
|
BgcVector3FP32 distance, momentum;
|
|
|
|
bgc_vector3_subtract_fp32(point, &motor->point, &distance);
|
|
bgc_vector3_get_cross_product_fp32(&distance, vector, &momentum);
|
|
|
|
bgc_vector3_subtract_scaled_fp32(&motor->vector, vector, scale, &motor->vector);
|
|
bgc_vector3_subtract_scaled_fp32(&motor->momentum, &momentum, scale, &motor->momentum);
|
|
}
|
|
|
|
inline void bt_motor_subtract_scaled_vector_fp64(const BgcVector3FP64* point, const BgcVector3FP64* vector, const double scale, BtMotorFP64* motor)
|
|
{
|
|
BgcVector3FP64 distance, momentum;
|
|
|
|
bgc_vector3_subtract_fp64(point, &motor->point, &distance);
|
|
bgc_vector3_get_cross_product_fp64(&distance, vector, &momentum);
|
|
|
|
bgc_vector3_subtract_scaled_fp64(&motor->vector, vector, scale, &motor->vector);
|
|
bgc_vector3_subtract_scaled_fp64(&motor->momentum, &momentum, scale, &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 Screw ================== //
|
|
|
|
inline int bt_motor_is_screw_fp32(const BtMotorFP32* motor)
|
|
{
|
|
return bgc_vector3_are_parallel_fp32(&motor->vector, &motor->momentum);
|
|
}
|
|
|
|
inline int bt_motor_is_screw_fp64(const BtMotorFP64* motor)
|
|
{
|
|
return bgc_vector3_are_parallel_fp64(&motor->vector, &motor->momentum);
|
|
}
|
|
|
|
// ================= Make Screw ================= //
|
|
|
|
inline void bt_motor_make_screw_fp32(BtMotorFP32* motor)
|
|
{
|
|
const float square_vector = bgc_vector3_get_square_modulus_fp32(&motor->vector);
|
|
|
|
if (square_vector <= BGC_SQUARE_EPSYLON_FP32) {
|
|
return;
|
|
}
|
|
|
|
const float multiplier = 1.0f / square_vector;
|
|
BgcVector3FP32 cross_product;
|
|
|
|
bgc_vector3_get_cross_product_fp32(&motor->vector, &motor->momentum, &cross_product);
|
|
|
|
bgc_vector3_add_scaled_fp32(&motor->point, &cross_product, multiplier, &motor->point);
|
|
|
|
bgc_vector3_multiply_fp32(&motor->vector, multiplier * bgc_vector3_get_scalar_product_fp32(&motor->vector, &motor->momentum), &motor->momentum);
|
|
}
|
|
|
|
inline void bt_motor_make_screw_fp64(BtMotorFP64* motor)
|
|
{
|
|
const double square_vector = bgc_vector3_get_square_modulus_fp64(&motor->vector);
|
|
|
|
if (square_vector <= BGC_SQUARE_EPSYLON_FP64) {
|
|
return;
|
|
}
|
|
|
|
const double multiplier = 1.0 / square_vector;
|
|
BgcVector3FP64 cross_product;
|
|
|
|
bgc_vector3_get_cross_product_fp64(&motor->vector, &motor->momentum, &cross_product);
|
|
|
|
bgc_vector3_add_scaled_fp64(&motor->point, &cross_product, multiplier, &motor->point);
|
|
|
|
bgc_vector3_multiply_fp64(&motor->vector, multiplier * bgc_vector3_get_scalar_product_fp64(&motor->vector, &motor->momentum), &motor->momentum);
|
|
}
|
|
|
|
// ================= Get Screw ================== //
|
|
|
|
inline void bt_motor_get_screw_fp32(const BtMotorFP32* motor, BtMotorFP32* screw)
|
|
{
|
|
const float square_vector = bgc_vector3_get_square_modulus_fp32(&motor->vector);
|
|
|
|
bgc_vector3_copy_fp32(&motor->vector, &screw->vector);
|
|
|
|
if (square_vector <= BGC_SQUARE_EPSYLON_FP32) {
|
|
bgc_vector3_copy_fp32(&motor->point, &screw->point);
|
|
bgc_vector3_copy_fp32(&motor->momentum, &screw->momentum);
|
|
return;
|
|
}
|
|
|
|
const float multiplier = 1.0f / square_vector;
|
|
BgcVector3FP32 cross_product;
|
|
|
|
bgc_vector3_get_cross_product_fp32(&motor->vector, &motor->momentum, &cross_product);
|
|
|
|
bgc_vector3_add_scaled_fp32(&motor->point, &cross_product, multiplier, &screw->point);
|
|
|
|
bgc_vector3_multiply_fp32(&motor->vector, multiplier * bgc_vector3_get_scalar_product_fp32(&motor->vector, &motor->momentum), &screw->momentum);
|
|
}
|
|
|
|
inline void bt_motor_get_screw_fp64(const BtMotorFP64* motor, BtMotorFP64* screw)
|
|
{
|
|
const double square_vector = bgc_vector3_get_square_modulus_fp64(&motor->vector);
|
|
|
|
bgc_vector3_copy_fp64(&motor->vector, &screw->vector);
|
|
|
|
if (square_vector <= BGC_SQUARE_EPSYLON_FP64) {
|
|
bgc_vector3_copy_fp64(&motor->point, &screw->point);
|
|
bgc_vector3_copy_fp64(&motor->momentum, &screw->momentum);
|
|
return;
|
|
}
|
|
|
|
const double multiplier = 1.0 / square_vector;
|
|
BgcVector3FP64 cross_product;
|
|
|
|
bgc_vector3_get_cross_product_fp64(&motor->vector, &motor->momentum, &cross_product);
|
|
|
|
bgc_vector3_add_scaled_fp64(&motor->point, &cross_product, multiplier, &screw->point);
|
|
|
|
bgc_vector3_multiply_fp64(&motor->vector, multiplier * bgc_vector3_get_scalar_product_fp64(&motor->vector, &motor->momentum), &screw->momentum);
|
|
}
|
|
|
|
#endif
|