Kimelas/Butis-C
2
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Переделка реализации мОтора

Browse source
This commit is contained in:
Andrey Pokidov 2025-12-25 20:56:52 +07:00
parent 43ce51a386
commit b85a554cb8
4 changed files with 508 additions and 388 deletions
Download patch file Download diff file Expand all files Collapse all files

9
Butis-C.workspace Normal file
View file

@ -0,0 +1,9 @@
<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
<CodeBlocks_workspace_file>
<Workspace title="Workspace">
<Project filename="butis/butis.cbp">
<Depends filename="basic-geometry/basic-geometry.cbp" />
</Project>
<Project filename="basic-geometry/basic-geometry.cbp" />
</Workspace>
</CodeBlocks_workspace_file>

55
butis/butis.cbp Normal file
View file

@ -0,0 +1,55 @@
<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
<CodeBlocks_project_file>
<FileVersion major="1" minor="6" />
<Project>
<Option title="butis" />
<Option pch_mode="2" />
<Option compiler="gcc" />
<Build>
<Target title="Debug">
<Option output="bin/Debug/butis" prefix_auto="1" extension_auto="1" />
<Option working_dir="" />
<Option object_output="obj/Debug/" />
<Option type="2" />
<Option compiler="gcc" />
<Option createDefFile="1" />
<Compiler>
<Add option="-Wall" />
<Add option="-g" />
<Add directory="../basic-geometry" />
</Compiler>
<Linker>
<Add library="basic-geometry" />
<Add directory="../basic-geometry/bin/Debug" />
</Linker>
</Target>
<Target title="Release">
<Option output="bin/Release/butis" prefix_auto="1" extension_auto="1" />
<Option working_dir="" />
<Option object_output="obj/Release/" />
<Option type="2" />
<Option compiler="gcc" />
<Option createDefFile="1" />
<Compiler>
<Add option="-O2" />
<Add option="-Wall" />
<Add directory="../basic-geometry" />
</Compiler>
<Linker>
<Add option="-s" />
<Add library="basic-geometry" />
<Add directory="../basic-geometry/bin/Release" />
</Linker>
</Target>
</Build>
<Unit filename="butis.c">
<Option compilerVar="CC" />
</Unit>
<Unit filename="butis.h" />
<Unit filename="motor.c">
<Option compilerVar="CC" />
</Unit>
<Unit filename="motor.h" />
<Extensions />
</Project>
</CodeBlocks_project_file>

98
butis/motor.c
View file

@ -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);

726
butis/motor.h
View file

@ -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);
if (square_vector <= BGC_SQUARE_EPSYLON_FP32) {
return;
bgc_vector3_copy_fp32(vector, &motor->vector);
bgc_vector3_copy_fp32(momentum, &motor->momentum);
}
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)
inline void bt_motor_set_fp64(const BgcVector3FP64* vector, const BgcVector3FP64* momentum, BtMotorFP64* motor)
{
const double square_vector = bgc_vector3_get_square_modulus_fp64(&motor->vector);
if (square_vector <= BGC_SQUARE_EPSYLON_FP64) {
return;
bgc_vector3_copy_fp64(vector, &motor->vector);
bgc_vector3_copy_fp64(momentum, &motor->momentum);
}
const double multiplier = 1.0 / square_vector;
BgcVector3FP64 cross_product;
// ================= Set Vector ================= //
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)
inline void bt_motor_set_vector_fp32(const BgcVector3FP32* vector, BtMotorFP32* motor)
{
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;
bgc_vector3_copy_fp32(vector, &motor->vector);
}
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)
inline void bt_motor_set_vector_fp64(const BgcVector3FP64* vector, BtMotorFP64* motor)
{
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;
bgc_vector3_copy_fp64(vector, &motor->vector);
}
const double multiplier = 1.0 / square_vector;
BgcVector3FP64 cross_product;
// ================ Set Momentum ================ //
bgc_vector3_get_cross_product_fp64(&motor->vector, &motor->momentum, &cross_product);
inline void bt_motor_set_momentum_fp32(const BgcVector3FP32* momentum, BtMotorFP32* motor)
{
bgc_vector3_copy_fp32(momentum, &motor->momentum);
}
bgc_vector3_add_scaled_fp64(&motor->point, &cross_product, multiplier, &screw->point);
inline void bt_motor_set_momentum_fp64(const BgcVector3FP64* momentum, BtMotorFP64* motor)
{
bgc_vector3_copy_fp64(momentum, &motor->momentum);
}
bgc_vector3_multiply_fp64(&motor->vector, multiplier * bgc_vector3_get_scalar_product_fp64(&motor->vector, &motor->momentum), &screw->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