Переделка реализации мОтора
This commit is contained in:
parent
43ce51a386
commit
b85a554cb8
4 changed files with 508 additions and 388 deletions
9
Butis-C.workspace
Normal file
9
Butis-C.workspace
Normal 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
55
butis/butis.cbp
Normal 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>
|
||||||
|
|
@ -3,56 +3,86 @@
|
||||||
extern inline void bt_motor_reset_fp32(BtMotorFP32* motor);
|
extern inline void bt_motor_reset_fp32(BtMotorFP32* motor);
|
||||||
extern inline void bt_motor_reset_fp64(BtMotorFP64* 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 int bt_motor_is_vector_fp32(const BtMotorFP32* motor);
|
||||||
extern inline void bt_motor_reset_at_point_fp64(const BgcVector3FP64* point, BtMotorFP64* 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 int bt_motor_is_momentum_fp32(const 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_fp64(const BtMotorFP64* motor);
|
||||||
|
|
||||||
extern inline void bt_motor_copy_fp32(const BtMotorFP32* source, BtMotorFP32* destination);
|
extern inline void bt_motor_set_fp32(const BgcVector3FP32* vector, const BgcVector3FP32* momentum, BtMotorFP32* motor);
|
||||||
extern inline void bt_motor_copy_fp64(const BtMotorFP64* source, BtMotorFP64* destination);
|
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_set_vector_fp32(const BgcVector3FP32* vector, BtMotorFP32* motor);
|
||||||
extern inline void bt_motor_convert_fp64_to_fp32(const BtMotorFP64* source, BtMotorFP32* destination);
|
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_fp32(BtMotorFP32* motor1, BtMotorFP32* motor2);
|
||||||
extern inline void bt_motor_swap_fp64(BtMotorFP64* motor1, BtMotorFP64* 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_fp32(const BgcVector3FP32* shift, BtMotorFP32* motor);
|
||||||
extern inline void bt_motor_shift_at_fp64(const BtMotorFP64* motor, const BgcVector3FP64* shift, BtMotorFP64* moved);
|
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_get_shifted_fp32(const BtMotorFP32* motor, const BgcVector3FP32* shift, BtMotorFP32* shifted);
|
||||||
extern inline void bt_motor_shift_to_fp64(const BtMotorFP64* motor, const BgcVector3FP64* destination, BtMotorFP64* moved);
|
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_add_fp32(const BtMotorFP32* motor1, const BtMotorFP32* motor2, BtMotorFP32* sum);
|
||||||
extern inline void bt_motor_multiply_at_number_fp64(const BtMotorFP64* multiplicand, const double multiplier, BtMotorFP64* product);
|
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_add_scaled_fp32(const BtMotorFP32* basic, const BtMotorFP32* scalable, const float scale, BtMotorFP32* sum);
|
||||||
extern inline void bt_motor_divide_at_number_fp64(const BtMotorFP64* divident, const double divisor, BtMotorFP64* quotient);
|
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_fp32(const BtMotorFP32* basic, 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_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_scaled_vector_fp32(const BtMotorFP32* basic, const BgcVector3FP32* point, const BgcVector3FP32* vector, const float scale, BtMotorFP32* sum);
|
||||||
extern inline void bt_motor_add_motor_fp64(const BtMotorFP64* motor, BtMotorFP64* 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_momentum_fp32(const BtMotorFP32* basic, const BgcVector3FP32* momentum, 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_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_add_scaled_momentum_fp32(const BtMotorFP32* basic, const BgcVector3FP32* momentum, const float scale, BtMotorFP32* sum);
|
||||||
extern inline void bt_motor_subtract_vector_fp64(const BgcVector3FP64* point, const BgcVector3FP64* vector, BtMotorFP64* difference);
|
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_fp32(const BtMotorFP32* minuend, const BtMotorFP32* subtrahend, 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_fp64(const BtMotorFP64* minuend, const BtMotorFP64* subtrahend, BtMotorFP64* difference);
|
||||||
|
|
||||||
extern inline int bt_motor_is_vector_fp32(const BtMotorFP32* motor);
|
extern inline void bt_motor_subtract_vector_fp32(const BtMotorFP32* minuend, const BgcVector3FP32* point, const BgcVector3FP32* vector, BtMotorFP32* difference);
|
||||||
extern inline int bt_motor_is_vector_fp64(const BtMotorFP64* motor);
|
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 void bt_motor_subtract_momentum_fp32(const BtMotorFP32* minuend, const BgcVector3FP32* momentum, BtMotorFP32* difference);
|
||||||
extern inline int bt_motor_is_screw_fp64(const BtMotorFP64* motor);
|
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_multiply_fp32(const BtMotorFP32* multiplicand, const float multiplier, BtMotorFP32* product);
|
||||||
extern inline void bt_motor_make_screw_fp64(BtMotorFP64* motor);
|
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_divide_fp32(const BtMotorFP32* dividend, const float divisor, BtMotorFP32* quotient);
|
||||||
extern inline void bt_motor_get_screw_fp64(const BtMotorFP64* motor, BtMotorFP64* screw);
|
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);
|
||||||
|
|
|
||||||
734
butis/motor.h
734
butis/motor.h
|
|
@ -6,305 +6,27 @@
|
||||||
// =================== Types ==================== //
|
// =================== Types ==================== //
|
||||||
|
|
||||||
typedef struct {
|
typedef struct {
|
||||||
BgcVector3FP32 point, vector, momentum;
|
BgcVector3FP32 vector, momentum;
|
||||||
} BtMotorFP32;
|
} BtMotorFP32;
|
||||||
|
|
||||||
typedef struct {
|
typedef struct {
|
||||||
BgcVector3FP64 point, vector, momentum;
|
BgcVector3FP64 vector, momentum;
|
||||||
} BtMotorFP64;
|
} BtMotorFP64;
|
||||||
|
|
||||||
// =================== Reset ==================== //
|
// =================== Reset ==================== //
|
||||||
|
|
||||||
inline void bt_motor_reset_fp32(BtMotorFP32* motor)
|
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->vector);
|
||||||
bgc_vector3_reset_fp32(&motor->momentum);
|
bgc_vector3_reset_fp32(&motor->momentum);
|
||||||
}
|
}
|
||||||
|
|
||||||
inline void bt_motor_reset_fp64(BtMotorFP64* motor)
|
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->vector);
|
||||||
bgc_vector3_reset_fp64(&motor->momentum);
|
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 ================== //
|
// ================= Is Vector ================== //
|
||||||
|
|
||||||
inline int bt_motor_is_vector_fp32(const BtMotorFP32* motor)
|
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);
|
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) {
|
inline void bt_motor_set_fp64(const BgcVector3FP64* vector, const BgcVector3FP64* momentum, BtMotorFP64* motor)
|
||||||
return;
|
{
|
||||||
|
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;
|
bgc_vector3_get_cross_product_fp32(&motor->vector, &motor->momentum, point);
|
||||||
BgcVector3FP32 cross_product;
|
bgc_vector3_divide_fp32(point, square_modulus, point);
|
||||||
|
|
||||||
bgc_vector3_get_cross_product_fp32(&motor->vector, &motor->momentum, &cross_product);
|
return 1;
|
||||||
|
|
||||||
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 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) {
|
if (square_modulus <= BGC_SQUARE_EPSYLON_FP64) {
|
||||||
return;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
const double multiplier = 1.0 / square_vector;
|
bgc_vector3_get_cross_product_fp64(&motor->vector, &motor->momentum, point);
|
||||||
BgcVector3FP64 cross_product;
|
bgc_vector3_divide_fp64(point, square_modulus, point);
|
||||||
|
|
||||||
bgc_vector3_get_cross_product_fp64(&motor->vector, &motor->momentum, &cross_product);
|
return 1;
|
||||||
|
|
||||||
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
|
#endif
|
||||||
|
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue