diff --git a/Butis-C.workspace b/Butis-C.workspace
new file mode 100644
index 0000000..e91d502
--- /dev/null
+++ b/Butis-C.workspace
@@ -0,0 +1,9 @@
+
+
+
+
+
+
+
+
+
diff --git a/butis/butis.cbp b/butis/butis.cbp
new file mode 100644
index 0000000..1e0d03a
--- /dev/null
+++ b/butis/butis.cbp
@@ -0,0 +1,55 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/butis/motor.c b/butis/motor.c
index ac58a12..ded03d0 100644
--- a/butis/motor.c
+++ b/butis/motor.c
@@ -3,56 +3,86 @@
extern inline void bt_motor_reset_fp32(BtMotorFP32* motor);
extern inline void bt_motor_reset_fp64(BtMotorFP64* motor);
-extern inline void bt_motor_reset_at_point_fp32(const BgcVector3FP32* point, BtMotorFP32* motor);
-extern inline void bt_motor_reset_at_point_fp64(const BgcVector3FP64* point, BtMotorFP64* motor);
+extern inline int bt_motor_is_vector_fp32(const BtMotorFP32* motor);
+extern inline int bt_motor_is_vector_fp64(const BtMotorFP64* motor);
-extern inline void bt_motor_reset_with_values_fp32(const float x1, const float x2, const float x3, BtMotorFP32* motor);
-extern inline void bt_motor_reset_with_values_fp64(const double x1, const double x2, const double x3, BtMotorFP64* motor);
+extern inline int bt_motor_is_momentum_fp32(const BtMotorFP32* motor);
+extern inline int bt_motor_is_momentum_fp64(const BtMotorFP64* motor);
-extern inline void bt_motor_copy_fp32(const BtMotorFP32* source, BtMotorFP32* destination);
-extern inline void bt_motor_copy_fp64(const BtMotorFP64* source, BtMotorFP64* destination);
+extern inline void bt_motor_set_fp32(const BgcVector3FP32* vector, const BgcVector3FP32* momentum, BtMotorFP32* motor);
+extern inline void bt_motor_set_fp64(const BgcVector3FP64* vector, const BgcVector3FP64* momentum, BtMotorFP64* motor);
-extern inline void bt_motor_convert_fp32_to_fp64(const BtMotorFP32* source, BtMotorFP64* destination);
-extern inline void bt_motor_convert_fp64_to_fp32(const BtMotorFP64* source, BtMotorFP32* destination);
+extern inline void bt_motor_set_vector_fp32(const BgcVector3FP32* vector, BtMotorFP32* motor);
+extern inline void bt_motor_set_vector_fp64(const BgcVector3FP64* vector, BtMotorFP64* motor);
+
+extern inline void bt_motor_set_momentum_fp32(const BgcVector3FP32* momentum, BtMotorFP32* motor);
+extern inline void bt_motor_set_momentum_fp64(const BgcVector3FP64* momentum, BtMotorFP64* motor);
+
+extern 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);
+extern 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);
+
+extern inline void bt_motor_make_fp32(const BgcVector3FP32* point, const BgcVector3FP32* vector, const BgcVector3FP32* momentum, BtMotorFP32* motor);
+extern inline void bt_motor_make_fp64(const BgcVector3FP64* point, const BgcVector3FP64* vector, const BgcVector3FP64* momentum, BtMotorFP64* motor);
+
+extern 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);
+
+extern 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);
+
+extern inline void bt_motor_make_pure_vector_fp32(const BgcVector3FP32* point, const BgcVector3FP32* vector, BtMotorFP32* motor);
+extern inline void bt_motor_make_pure_vector_fp64(const BgcVector3FP64* point, const BgcVector3FP64* vector, BtMotorFP64* motor);
+
+extern inline void bt_motor_make_pure_momentum_fp32(const BgcVector3FP32* momentum, BtMotorFP32* motor);
+extern inline void bt_motor_make_pure_momentum_fp64(const BgcVector3FP64* momentum, BtMotorFP64* motor);
extern inline void bt_motor_swap_fp32(BtMotorFP32* motor1, BtMotorFP32* motor2);
extern inline void bt_motor_swap_fp64(BtMotorFP64* motor1, BtMotorFP64* motor2);
-extern inline void bt_motor_shift_at_fp32(const BtMotorFP32* motor, const BgcVector3FP32* shift, BtMotorFP32* moved);
-extern inline void bt_motor_shift_at_fp64(const BtMotorFP64* motor, const BgcVector3FP64* shift, BtMotorFP64* moved);
+extern inline void bt_motor_shift_fp32(const BgcVector3FP32* shift, BtMotorFP32* motor);
+extern inline void bt_motor_shift_fp64(const BgcVector3FP64* shift, BtMotorFP64* motor);
-extern inline void bt_motor_shift_to_fp32(const BtMotorFP32* motor, const BgcVector3FP32* destination, BtMotorFP32* moved);
-extern inline void bt_motor_shift_to_fp64(const BtMotorFP64* motor, const BgcVector3FP64* destination, BtMotorFP64* moved);
+extern inline void bt_motor_get_shifted_fp32(const BtMotorFP32* motor, const BgcVector3FP32* shift, BtMotorFP32* shifted);
+extern inline void bt_motor_get_shifted_fp64(const BtMotorFP64* motor, const BgcVector3FP64* shift, BtMotorFP64* shifted);
-extern inline void bt_motor_multiply_at_number_fp32(const BtMotorFP32* multiplicand, const float multiplier, BtMotorFP32* product);
-extern inline void bt_motor_multiply_at_number_fp64(const BtMotorFP64* multiplicand, const double multiplier, BtMotorFP64* product);
+extern inline void bt_motor_add_fp32(const BtMotorFP32* motor1, const BtMotorFP32* motor2, BtMotorFP32* sum);
+extern inline void bt_motor_add_fp64(const BtMotorFP64* motor1, const BtMotorFP64* motor2, BtMotorFP64* sum);
-extern inline void bt_motor_divide_at_number_fp32(const BtMotorFP32* divident, const float divisor, BtMotorFP32* quotient);
-extern inline void bt_motor_divide_at_number_fp64(const BtMotorFP64* divident, const double divisor, BtMotorFP64* quotient);
+extern inline void bt_motor_add_scaled_fp32(const BtMotorFP32* basic, const BtMotorFP32* scalable, const float scale, BtMotorFP32* sum);
+extern inline void bt_motor_add_scaled_fp64(const BtMotorFP64* basic, const BtMotorFP64* scalable, const double scale, BtMotorFP64* sum);
-extern inline void bt_motor_add_vector_fp32(const BgcVector3FP32* point, const BgcVector3FP32* vector, BtMotorFP32* sum);
-extern inline void bt_motor_add_vector_fp64(const BgcVector3FP64* point, const BgcVector3FP64* vector, BtMotorFP64* sum);
+extern inline void bt_motor_add_vector_fp32(const BtMotorFP32* basic, const BgcVector3FP32* point, const BgcVector3FP32* vector, BtMotorFP32* sum);
+extern inline void bt_motor_add_vector_fp64(const BtMotorFP64* basic, const BgcVector3FP64* point, const BgcVector3FP64* vector, BtMotorFP64* sum);
-extern inline void bt_motor_add_motor_fp32(const BtMotorFP32* motor, BtMotorFP32* sum);
-extern inline void bt_motor_add_motor_fp64(const BtMotorFP64* motor, BtMotorFP64* sum);
+extern inline void bt_motor_add_scaled_vector_fp32(const BtMotorFP32* basic, const BgcVector3FP32* point, const BgcVector3FP32* vector, const float scale, BtMotorFP32* sum);
+extern inline void bt_motor_add_scaled_vector_fp64(const BtMotorFP64* basic, const BgcVector3FP64* point, const BgcVector3FP64* vector, const double scale, BtMotorFP64* sum);
-extern inline void bt_motor_add_scaled_vector_fp32(const BgcVector3FP32* point, const BgcVector3FP32* vector, const float scale, BtMotorFP32* sum);
-extern inline void bt_motor_add_scaled_vector_fp64(const BgcVector3FP64* point, const BgcVector3FP64* vector, const double scale, BtMotorFP64* sum);
+extern inline void bt_motor_add_momentum_fp32(const BtMotorFP32* basic, const BgcVector3FP32* momentum, BtMotorFP32* sum);
+extern inline void bt_motor_add_momentum_fp64(const BtMotorFP64* basic, const BgcVector3FP64* momentum, BtMotorFP64* sum);
-extern inline void bt_motor_subtract_vector_fp32(const BgcVector3FP32* point, const BgcVector3FP32* vector, BtMotorFP32* difference);
-extern inline void bt_motor_subtract_vector_fp64(const BgcVector3FP64* point, const BgcVector3FP64* vector, BtMotorFP64* difference);
+extern inline void bt_motor_add_scaled_momentum_fp32(const BtMotorFP32* basic, const BgcVector3FP32* momentum, const float scale, BtMotorFP32* sum);
+extern inline void bt_motor_add_scaled_momentum_fp64(const BtMotorFP64* basic, const BgcVector3FP64* momentum, const double scale, BtMotorFP64* sum);
-extern inline void bt_motor_subtract_scaled_vector_fp32(const BgcVector3FP32* point, const BgcVector3FP32* vector, const float scale, BtMotorFP32* difference);
-extern inline void bt_motor_subtract_scaled_vector_fp64(const BgcVector3FP64* point, const BgcVector3FP64* vector, const double scale, BtMotorFP64* differences);
+extern inline void bt_motor_subtract_fp32(const BtMotorFP32* minuend, const BtMotorFP32* subtrahend, BtMotorFP32* difference);
+extern inline void bt_motor_subtract_fp64(const BtMotorFP64* minuend, const BtMotorFP64* subtrahend, BtMotorFP64* difference);
-extern inline int bt_motor_is_vector_fp32(const BtMotorFP32* motor);
-extern inline int bt_motor_is_vector_fp64(const BtMotorFP64* motor);
+extern inline void bt_motor_subtract_vector_fp32(const BtMotorFP32* minuend, const BgcVector3FP32* point, const BgcVector3FP32* vector, BtMotorFP32* difference);
+extern inline void bt_motor_subtract_vector_fp64(const BtMotorFP64* minuend, const BgcVector3FP64* point, const BgcVector3FP64* vector, BtMotorFP64* difference);
-extern inline int bt_motor_is_screw_fp32(const BtMotorFP32* motor);
-extern inline int bt_motor_is_screw_fp64(const BtMotorFP64* motor);
+extern inline void bt_motor_subtract_momentum_fp32(const BtMotorFP32* minuend, const BgcVector3FP32* momentum, BtMotorFP32* difference);
+extern inline void bt_motor_subtract_momentum_fp64(const BtMotorFP64* minuend, const BgcVector3FP64* momentum, BtMotorFP64* difference);
-extern inline void bt_motor_make_screw_fp32(BtMotorFP32* motor);
-extern inline void bt_motor_make_screw_fp64(BtMotorFP64* motor);
+extern inline void bt_motor_multiply_fp32(const BtMotorFP32* multiplicand, const float multiplier, BtMotorFP32* product);
+extern inline void bt_motor_multiply_fp64(const BtMotorFP64* multiplicand, const double multiplier, BtMotorFP64* product);
-extern inline void bt_motor_get_screw_fp32(const BtMotorFP32* motor, BtMotorFP32* screw);
-extern inline void bt_motor_get_screw_fp64(const BtMotorFP64* motor, BtMotorFP64* screw);
+extern inline void bt_motor_divide_fp32(const BtMotorFP32* dividend, const float divisor, BtMotorFP32* quotient);
+extern inline void bt_motor_divide_fp64(const BtMotorFP64* dividend, const double divisor, BtMotorFP64* quotient);
+
+extern inline int bt_motor_get_central_point_fp32(const BtMotorFP32* motor, BgcVector3FP32* point);
+extern inline int bt_motor_get_central_point_fp64(const BtMotorFP64* motor, BgcVector3FP64* point);
diff --git a/butis/motor.h b/butis/motor.h
index 8fb6ba2..33a42ba 100644
--- a/butis/motor.h
+++ b/butis/motor.h
@@ -6,305 +6,27 @@
// =================== Types ==================== //
typedef struct {
- BgcVector3FP32 point, vector, momentum;
+ BgcVector3FP32 vector, momentum;
} BtMotorFP32;
typedef struct {
- BgcVector3FP64 point, vector, momentum;
+ BgcVector3FP64 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)
@@ -317,100 +39,404 @@ inline int bt_motor_is_vector_fp64(const BtMotorFP64* motor)
return bgc_vector3_are_orthogonal_fp64(&motor->vector, &motor->momentum);
}
-// ================== Is Screw ================== //
+// ============== Is Pure Momentum ============== //
-inline int bt_motor_is_screw_fp32(const BtMotorFP32* motor)
+inline int bt_motor_is_momentum_fp32(const BtMotorFP32* motor)
{
- return bgc_vector3_are_parallel_fp32(&motor->vector, &motor->momentum);
+ return bgc_vector3_is_zero_fp32(&motor->vector);
}
-inline int bt_motor_is_screw_fp64(const BtMotorFP64* motor)
+inline int bt_motor_is_momentum_fp64(const BtMotorFP64* motor)
{
- return bgc_vector3_are_parallel_fp64(&motor->vector, &motor->momentum);
+ return bgc_vector3_is_zero_fp64(&motor->vector);
}
-// ================= Make Screw ================= //
+// ==================== Set ===================== //
-inline void bt_motor_make_screw_fp32(BtMotorFP32* motor)
+inline void bt_motor_set_fp32(const BgcVector3FP32* vector, const BgcVector3FP32* momentum, BtMotorFP32* motor)
{
- const float square_vector = bgc_vector3_get_square_modulus_fp32(&motor->vector);
+ bgc_vector3_copy_fp32(vector, &motor->vector);
+ bgc_vector3_copy_fp32(momentum, &motor->momentum);
+}
- if (square_vector <= BGC_SQUARE_EPSYLON_FP32) {
- return;
+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;
}
- const float multiplier = 1.0f / square_vector;
- BgcVector3FP32 cross_product;
+ bgc_vector3_get_cross_product_fp32(&motor->vector, &motor->momentum, point);
+ bgc_vector3_divide_fp32(point, square_modulus, point);
- 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);
+ return 1;
}
-inline void bt_motor_make_screw_fp64(BtMotorFP64* motor)
+inline int bt_motor_get_central_point_fp64(const BtMotorFP64* motor, BgcVector3FP64* point)
{
- const double square_vector = bgc_vector3_get_square_modulus_fp64(&motor->vector);
+ const double square_modulus = bgc_vector3_get_square_modulus_fp64(&motor->vector);
- if (square_vector <= BGC_SQUARE_EPSYLON_FP64) {
- return;
+ if (square_modulus <= BGC_SQUARE_EPSYLON_FP64) {
+ return 0;
}
- const double multiplier = 1.0 / square_vector;
- BgcVector3FP64 cross_product;
+ bgc_vector3_get_cross_product_fp64(&motor->vector, &motor->momentum, point);
+ bgc_vector3_divide_fp64(point, square_modulus, point);
- 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);
+ return 1;
}
#endif