Сложение моторов, формирование систем
This commit is contained in:
parent
b56deff0b0
commit
43ce51a386
7 changed files with 193 additions and 60 deletions
168
butis/motor.h
168
butis/motor.h
|
|
@ -29,6 +29,38 @@ inline void bt_motor_reset_fp64(BtMotorFP64* motor)
|
|||
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)
|
||||
|
|
@ -149,6 +181,142 @@ inline void bt_motor_divide_at_number_fp64(const BtMotorFP64* divident, const do
|
|||
bt_motor_multiply_at_number_fp64(divident, 1.0 / divisor, quotient);
|
||||
}
|
||||
|
||||
// ==================== Add ===================== //
|
||||
|
||||
inline void bt_motor_add_vector_fp32(const BgcVector3FP32* point, const BgcVector3FP32* vector, BtMotorFP32* sum)
|
||||
{
|
||||
BgcVector3FP32 distance, momentum;
|
||||
|
||||
bgc_vector3_subtract_fp32(point, &sum->point, &distance);
|
||||
bgc_vector3_get_cross_product_fp32(&distance, vector, &momentum);
|
||||
|
||||
bgc_vector3_add_fp32(&sum->vector, vector, &sum->vector);
|
||||
bgc_vector3_add_fp32(&sum->momentum, &momentum, &sum->momentum);
|
||||
}
|
||||
|
||||
inline void bt_motor_add_vector_fp64(const BgcVector3FP64* point, const BgcVector3FP64* vector, BtMotorFP64* sum)
|
||||
{
|
||||
BgcVector3FP64 distance, momentum;
|
||||
|
||||
bgc_vector3_subtract_fp64(point, &sum->point, &distance);
|
||||
bgc_vector3_get_cross_product_fp64(&distance, vector, &momentum);
|
||||
|
||||
bgc_vector3_add_fp64(&sum->vector, vector, &sum->vector);
|
||||
bgc_vector3_add_fp64(&sum->momentum, &momentum, &sum->momentum);
|
||||
}
|
||||
|
||||
// ================= Add Motor ================== //
|
||||
|
||||
inline void bt_motor_add_motor_fp32(const BtMotorFP32* motor, BtMotorFP32* sum)
|
||||
{
|
||||
BgcVector3FP32 distance, momentum;
|
||||
|
||||
bgc_vector3_subtract_fp32(&motor->point, &sum->point, &distance);
|
||||
bgc_vector3_get_cross_product_fp32(&distance, &motor->vector, &momentum);
|
||||
|
||||
bgc_vector3_add_fp32(&momentum, &motor->vector, &momentum);
|
||||
|
||||
bgc_vector3_add_fp32(&sum->vector, &motor->vector, &sum->vector);
|
||||
bgc_vector3_add_fp32(&sum->momentum, &momentum, &sum->momentum);
|
||||
}
|
||||
|
||||
inline void bt_motor_add_motor_fp64(const BtMotorFP64* motor, BtMotorFP64* sum)
|
||||
{
|
||||
BgcVector3FP64 distance, momentum;
|
||||
|
||||
bgc_vector3_subtract_fp64(&motor->point, &sum->point, &distance);
|
||||
bgc_vector3_get_cross_product_fp64(&distance, &motor->vector, &momentum);
|
||||
|
||||
bgc_vector3_add_fp64(&momentum, &motor->vector, &momentum);
|
||||
|
||||
bgc_vector3_add_fp64(&sum->vector, &motor->vector, &sum->vector);
|
||||
bgc_vector3_add_fp64(&sum->momentum, &momentum, &sum->momentum);
|
||||
}
|
||||
|
||||
// ================= Add Scaled ================= //
|
||||
|
||||
inline void bt_motor_add_scaled_vector_fp32(const BgcVector3FP32* point, const BgcVector3FP32* vector, const float scale, BtMotorFP32* sum)
|
||||
{
|
||||
BgcVector3FP32 distance, momentum;
|
||||
|
||||
bgc_vector3_subtract_fp32(point, &sum->point, &distance);
|
||||
bgc_vector3_get_cross_product_fp32(&distance, vector, &momentum);
|
||||
|
||||
bgc_vector3_add_scaled_fp32(&sum->vector, vector, scale, &sum->vector);
|
||||
bgc_vector3_add_scaled_fp32(&sum->momentum, &momentum, scale, &sum->momentum);
|
||||
}
|
||||
|
||||
inline void bt_motor_add_scaled_vector_fp64(const BgcVector3FP64* point, const BgcVector3FP64* vector, const double scale, BtMotorFP64* sum)
|
||||
{
|
||||
BgcVector3FP64 distance, momentum;
|
||||
|
||||
bgc_vector3_subtract_fp64(point, &sum->point, &distance);
|
||||
bgc_vector3_get_cross_product_fp64(&distance, vector, &momentum);
|
||||
|
||||
bgc_vector3_add_scaled_fp64(&sum->vector, vector, scale, &sum->vector);
|
||||
bgc_vector3_add_scaled_fp64(&sum->momentum, &momentum, scale, &sum->momentum);
|
||||
}
|
||||
|
||||
// ================== Subtract ================== //
|
||||
|
||||
inline void bt_motor_subtract_vector_fp32(const BgcVector3FP32* point, const BgcVector3FP32* vector, BtMotorFP32* difference)
|
||||
{
|
||||
BgcVector3FP32 distance, momentum;
|
||||
|
||||
bgc_vector3_subtract_fp32(point, &difference->point, &distance);
|
||||
bgc_vector3_get_cross_product_fp32(&distance, vector, &momentum);
|
||||
|
||||
bgc_vector3_subtract_fp32(&difference->vector, vector, &difference->vector);
|
||||
bgc_vector3_subtract_fp32(&difference->momentum, &momentum, &difference->momentum);
|
||||
}
|
||||
|
||||
inline void bt_motor_subtract_vector_fp64(const BgcVector3FP64* point, const BgcVector3FP64* vector, BtMotorFP64* difference)
|
||||
{
|
||||
BgcVector3FP64 distance, momentum;
|
||||
|
||||
bgc_vector3_subtract_fp64(point, &difference->point, &distance);
|
||||
bgc_vector3_get_cross_product_fp64(&distance, vector, &momentum);
|
||||
|
||||
bgc_vector3_subtract_fp64(&difference->vector, vector, &difference->vector);
|
||||
bgc_vector3_subtract_fp64(&difference->momentum, &momentum, &difference->momentum);
|
||||
}
|
||||
|
||||
// ============== Subtract Scaled =============== //
|
||||
|
||||
inline void bt_motor_subtract_scaled_vector_fp32(const BgcVector3FP32* point, const BgcVector3FP32* vector, const float scale, BtMotorFP32* motor)
|
||||
{
|
||||
BgcVector3FP32 distance, momentum;
|
||||
|
||||
bgc_vector3_subtract_fp32(point, &motor->point, &distance);
|
||||
bgc_vector3_get_cross_product_fp32(&distance, vector, &momentum);
|
||||
|
||||
bgc_vector3_subtract_scaled_fp32(&motor->vector, vector, scale, &motor->vector);
|
||||
bgc_vector3_subtract_scaled_fp32(&motor->momentum, &momentum, scale, &motor->momentum);
|
||||
}
|
||||
|
||||
inline void bt_motor_subtract_scaled_vector_fp64(const BgcVector3FP64* point, const BgcVector3FP64* vector, const double scale, BtMotorFP64* motor)
|
||||
{
|
||||
BgcVector3FP64 distance, momentum;
|
||||
|
||||
bgc_vector3_subtract_fp64(point, &motor->point, &distance);
|
||||
bgc_vector3_get_cross_product_fp64(&distance, vector, &momentum);
|
||||
|
||||
bgc_vector3_subtract_scaled_fp64(&motor->vector, vector, scale, &motor->vector);
|
||||
bgc_vector3_subtract_scaled_fp64(&motor->momentum, &momentum, scale, &motor->momentum);
|
||||
}
|
||||
|
||||
// ================= Is Vector ================== //
|
||||
|
||||
inline int bt_motor_is_vector_fp32(const BtMotorFP32* motor)
|
||||
{
|
||||
return bgc_vector3_are_orthogonal_fp32(&motor->vector, &motor->momentum);
|
||||
}
|
||||
|
||||
inline int bt_motor_is_vector_fp64(const BtMotorFP64* motor)
|
||||
{
|
||||
return bgc_vector3_are_orthogonal_fp64(&motor->vector, &motor->momentum);
|
||||
}
|
||||
|
||||
// ================== Is Screw ================== //
|
||||
|
||||
inline int bt_motor_is_screw_fp32(const BtMotorFP32* motor)
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue