Сложение моторов, формирование систем

.gitignore

@@ -48,6 +48,7 @@
 *.cmd
 .tmp_versions/
 .vs
+basic-geometry
 x64
 x86
 Debug

butis/butis.vcxproj

@@ -162,13 +162,11 @@
     <ClInclude Include="butis.h" />
     <ClInclude Include="motor.h" />
     <ClInclude Include="motor_matrix.h" />
-    <ClInclude Include="screw.h" />
   </ItemGroup>
   <ItemGroup>
     <ClCompile Include="butis.c" />
     <ClCompile Include="motor.c" />
     <ClCompile Include="motor_matrix.c" />
-    <ClCompile Include="screw.c" />
   </ItemGroup>
   <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
   <ImportGroup Label="ExtensionTargets">

butis/butis.vcxproj.filters

@@ -24,9 +24,6 @@
     <ClInclude Include="motor_matrix.h">
       <Filter>Файлы заголовков</Filter>
     </ClInclude>
-    <ClInclude Include="screw.h">
-      <Filter>Файлы заголовков</Filter>
-    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <ClCompile Include="butis.c">
@@ -38,8 +35,5 @@
     <ClCompile Include="motor_matrix.c">
       <Filter>Исходные файлы</Filter>
     </ClCompile>
-    <ClCompile Include="screw.c">
-      <Filter>Исходные файлы</Filter>
-    </ClCompile>
   </ItemGroup>
 </Project>

butis/motor.c

@@ -3,6 +3,12 @@
 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 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 void bt_motor_copy_fp32(const BtMotorFP32* source, BtMotorFP32* destination);
 extern inline void bt_motor_copy_fp64(const BtMotorFP64* source, BtMotorFP64* destination);
 
@@ -24,6 +30,24 @@ extern inline void bt_motor_multiply_at_number_fp64(const BtMotorFP64* multiplic
 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_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_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 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_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_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 int bt_motor_is_vector_fp32(const BtMotorFP32* motor);
+extern inline int bt_motor_is_vector_fp64(const BtMotorFP64* motor);
+
 extern inline int bt_motor_is_screw_fp32(const BtMotorFP32* motor);
 extern inline int bt_motor_is_screw_fp64(const BtMotorFP64* motor);
 

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)

butis/screw.c

@@ -1,4 +0,0 @@
-#include "screw.h"
-
-extern inline void bt_screw_reset_fp32(BtScrewFP32* screw);
-extern inline void bt_screw_reset_fp64(BtScrewFP64* screw);

butis/screw.h

@@ -1,48 +0,0 @@
-#ifndef _BUTIS_SCREW_H_INCLUDE_
-#define _BUTIS_SCREW_H_INCLUDE_
-
-#include <basic-geometry.h>
-
-// =================== Types ==================== //
-
-typedef struct {
-    BgcVector3FP32 point, direction;
-    float value, momentum;
-} BtScrewFP32;
-
-typedef struct {
-    BgcVector3FP64 point, direction;
-    double value, momentum;
-} BtScrewFP64;
-
-// =================== Reset ==================== //
-
-inline void bt_screw_reset_fp32(BtScrewFP32* screw)
-{
-    screw->point.x1 = 0.0f;
-    screw->point.x2 = 0.0f;
-    screw->point.x3 = 0.0f;
-
-    screw->direction.x1 = 1.0f;
-    screw->direction.x2 = 0.0f;
-    screw->direction.x3 = 0.0f;
-
-    screw->value = 0.0f;
-    screw->momentum = 0.0f;
-}
-
-inline void bt_screw_reset_fp64(BtScrewFP64* screw)
-{
-    screw->point.x1 = 0.0;
-    screw->point.x2 = 0.0;
-    screw->point.x3 = 0.0;
-
-    screw->direction.x1 = 1.0;
-    screw->direction.x2 = 0.0;
-    screw->direction.x3 = 0.0;
-
-    screw->value = 0.0f;
-    screw->momentum = 0.0f;
-}
-
-#endif