Добавление комбинации двух aффинных преобразований

basic-geometry/affine3.c

@@ -3,9 +3,18 @@
 extern inline void bgc_affine3_reset_fp32(BgcAffine3FP32 * affine);
 extern inline void bgc_affine3_reset_fp64(BgcAffine3FP64 * affine);
 
+extern inline void bgc_affine3_reset_distortion_fp32(BgcAffine3FP32 * affine);
+extern inline void bgc_affine3_reset_distortion_fp64(BgcAffine3FP64 * affine);
+
+extern inline void bgc_affine3_reset_shift_fp32(BgcAffine3FP32 * affine);
+extern inline void bgc_affine3_reset_shift_fp64(BgcAffine3FP64 * affine);
+
 extern inline void bgc_affine3_make_fp32(const BgcMatrix3x3FP32 * distortion, const BgcVector3FP32 * shift, BgcAffine3FP32 * affine);
 extern inline void bgc_affine3_make_fp64(const BgcMatrix3x3FP64 * distortion, const BgcVector3FP64 * shift, BgcAffine3FP64 * affine);
 
+extern inline void bgc_affine3_combine_fp32(const BgcAffine3FP32 * parent, const BgcAffine3FP32 * child, BgcAffine3FP32 * combination);
+extern inline void bgc_affine3_combine_fp64(const BgcAffine3FP64 * parent, const BgcAffine3FP64 * child, BgcAffine3FP64 * combination);
+
 extern inline void bgc_affine3_transform_point_fp32(const BgcAffine3FP32 * affine, const BgcVector3FP32 * initial_point, BgcVector3FP32 * transformed_point);
 extern inline void bgc_affine3_transform_point_fp64(const BgcAffine3FP64 * affine, const BgcVector3FP64 * initial_point, BgcVector3FP64 * transformed_point);
 

basic-geometry/affine3.h

@@ -78,6 +78,66 @@ inline void bgc_affine3_reset_fp64(BgcAffine3FP64 * affine)
 #endif
 }
 
+inline void bgc_affine3_reset_distortion_fp32(BgcAffine3FP32 * affine)
+{
+#if BGC_AFFINE_USE_MATRIX
+    bgc_matrix3x3_set_to_identity_fp32(&affine->distortion);
+#else
+    affine->r1c1 = 1.0f;
+    affine->r1c2 = 0.0f;
+    affine->r1c3 = 0.0f;
+
+    affine->r2c1 = 0.0f;
+    affine->r2c2 = 1.0f;
+    affine->r2c3 = 0.0f;
+
+    affine->r3c1 = 0.0f;
+    affine->r3c2 = 0.0f;
+    affine->r3c3 = 1.0f;
+#endif
+}
+
+inline void bgc_affine3_reset_distortion_fp64(BgcAffine3FP64 * affine)
+{
+#if BGC_AFFINE_USE_MATRIX
+    bgc_matrix3x3_set_to_identity_fp64(&affine->distortion);
+#else
+    affine->r1c1 = 1.0;
+    affine->r1c2 = 0.0;
+    affine->r1c3 = 0.0;
+
+    affine->r2c1 = 0.0;
+    affine->r2c2 = 1.0;
+    affine->r2c3 = 0.0;
+
+    affine->r3c1 = 0.0;
+    affine->r3c2 = 0.0;
+    affine->r3c3 = 1.0;
+#endif
+}
+
+inline void bgc_affine3_reset_shift_fp32(BgcAffine3FP32 * affine)
+{
+#if BGC_AFFINE_USE_MATRIX
+    bgc_vector3_reset_fp32(&affine->shift);
+#else
+    affine->shift1 = 0.0f;
+    affine->shift2 = 0.0f;
+    affine->shift3 = 0.0f;
+#endif
+}
+
+inline void bgc_affine3_reset_shift_fp64(BgcAffine3FP64 * affine)
+{
+#if BGC_AFFINE_USE_MATRIX
+    bgc_vector3_reset_fp64(&affine->shift);
+#else
+    affine->shift1 = 0.0;
+    affine->shift2 = 0.0;
+    affine->shift3 = 0.0;
+#endif
+}
+
 // ==================== Make ===================== //
 
 inline void bgc_affine3_make_fp32(const BgcMatrix3x3FP32 * distortion, const BgcVector3FP32 * shift, BgcAffine3FP32 * affine)
@@ -126,6 +186,90 @@ inline void bgc_affine3_make_fp64(const BgcMatrix3x3FP64 * distortion, const Bgc
 #endif
 }
 
+// =================== Combine =================== //
+
+inline void bgc_affine3_combine_fp32(const BgcAffine3FP32 * parent, const BgcAffine3FP32 * child, BgcAffine3FP32 * combination)
+{
+#if BGC_AFFINE_USE_MATRIX
+    BgcVector3FP32 child_shift;
+    bgc_matrix3x3_get_right_product_fp32(&parent->distortion, &child->shift, &child_shift);
+    bgc_matrix_product_3x3_at_3x3_fp32(&parent->distortion, &child->distortion, &combination->distortion);
+    bgc_vector3_add_fp32(&parent->shift, &child_shift, &combination->shift);
+#else
+    const float r1c1 = parent->r1c1 * child->r1c1 + parent->r1c2 * child->r2c1 + parent->r1c3 * child->r3c1;
+    const float r1c2 = parent->r1c1 * child->r1c2 + parent->r1c2 * child->r2c2 + parent->r1c3 * child->r3c2;
+    const float r1c3 = parent->r1c1 * child->r1c3 + parent->r1c2 * child->r2c3 + parent->r1c3 * child->r3c3;
+
+    const float r2c1 = parent->r2c1 * child->r1c1 + parent->r2c2 * child->r2c1 + parent->r2c3 * child->r3c1;
+    const float r2c2 = parent->r2c1 * child->r1c2 + parent->r2c2 * child->r2c2 + parent->r2c3 * child->r3c2;
+    const float r2c3 = parent->r2c1 * child->r1c3 + parent->r2c2 * child->r2c3 + parent->r2c3 * child->r3c3;
+
+    const float r3c1 = parent->r3c1 * child->r1c1 + parent->r3c2 * child->r2c1 + parent->r3c3 * child->r3c1;
+    const float r3c2 = parent->r3c1 * child->r1c2 + parent->r3c2 * child->r2c2 + parent->r3c3 * child->r3c2;
+    const float r3c3 = parent->r3c1 * child->r1c3 + parent->r3c2 * child->r2c3 + parent->r3c3 * child->r3c3;
+
+    const float shift1 = parent->r1c1 * child->shift1 + parent->r1c2 * child->shift2 + parent->r1c3 * child->shift3;
+    const float shift2 = parent->r2c1 * child->shift1 + parent->r2c2 * child->shift2 + parent->r2c3 * child->shift3;
+    const float shift3 = parent->r3c1 * child->shift1 + parent->r3c2 * child->shift2 + parent->r3c3 * child->shift3;
+
+    combination->r1c1 = r1c1;
+    combination->r1c2 = r1c2;
+    combination->r1c3 = r1c3;
+    combination->shift1 = shift1;
+
+    combination->r2c1 = r2c1;
+    combination->r2c2 = r2c2;
+    combination->r2c3 = r2c3;
+    combination->shift2 = shift2;
+
+    combination->r3c1 = r3c1;
+    combination->r3c2 = r3c2;
+    combination->r3c3 = r3c3;
+    combination->shift3 = shift3;
+#endif
+}
+
+inline void bgc_affine3_combine_fp64(const BgcAffine3FP64 * parent, const BgcAffine3FP64 * child, BgcAffine3FP64 * combination)
+{
+#if BGC_AFFINE_USE_MATRIX
+    BgcVector3FP64 child_shift;
+    bgc_matrix3x3_get_right_product_fp64(&parent->distortion, &child->shift, &child_shift);
+    bgc_matrix_product_3x3_at_3x3_fp64(&parent->distortion, &child->distortion, &combination->distortion);
+    bgc_vector3_add_fp64(&parent->shift, &child_shift, &combination->shift);
+#else
+    const double r1c1 = parent->r1c1 * child->r1c1 + parent->r1c2 * child->r2c1 + parent->r1c3 * child->r3c1;
+    const double r1c2 = parent->r1c1 * child->r1c2 + parent->r1c2 * child->r2c2 + parent->r1c3 * child->r3c2;
+    const double r1c3 = parent->r1c1 * child->r1c3 + parent->r1c2 * child->r2c3 + parent->r1c3 * child->r3c3;
+
+    const double r2c1 = parent->r2c1 * child->r1c1 + parent->r2c2 * child->r2c1 + parent->r2c3 * child->r3c1;
+    const double r2c2 = parent->r2c1 * child->r1c2 + parent->r2c2 * child->r2c2 + parent->r2c3 * child->r3c2;
+    const double r2c3 = parent->r2c1 * child->r1c3 + parent->r2c2 * child->r2c3 + parent->r2c3 * child->r3c3;
+
+    const double r3c1 = parent->r3c1 * child->r1c1 + parent->r3c2 * child->r2c1 + parent->r3c3 * child->r3c1;
+    const double r3c2 = parent->r3c1 * child->r1c2 + parent->r3c2 * child->r2c2 + parent->r3c3 * child->r3c2;
+    const double r3c3 = parent->r3c1 * child->r1c3 + parent->r3c2 * child->r2c3 + parent->r3c3 * child->r3c3;
+
+    const double shift1 = parent->r1c1 * child->shift1 + parent->r1c2 * child->shift2 + parent->r1c3 * child->shift3;
+    const double shift2 = parent->r2c1 * child->shift1 + parent->r2c2 * child->shift2 + parent->r2c3 * child->shift3;
+    const double shift3 = parent->r3c1 * child->shift1 + parent->r3c2 * child->shift2 + parent->r3c3 * child->shift3;
+
+    combination->r1c1 = r1c1;
+    combination->r1c2 = r1c2;
+    combination->r1c3 = r1c3;
+    combination->shift1 = shift1;
+
+    combination->r2c1 = r2c1;
+    combination->r2c2 = r2c2;
+    combination->r2c3 = r2c3;
+    combination->shift2 = shift2;
+
+    combination->r3c1 = r3c1;
+    combination->r3c2 = r3c2;
+    combination->r3c3 = r3c3;
+    combination->shift3 = shift3;
+#endif
+}
+
 // =============== Transform Point =============== //
 
 inline void bgc_affine3_transform_point_fp32(const BgcAffine3FP32 * affine, const BgcVector3FP32 * initial_point, BgcVector3FP32 * transformed_point)

basic-geometry/utilities.h

@@ -47,7 +47,7 @@
 #define BGC_AXIS_REVERSE_X3 -3
 
 #if !defined(BGC_AFFINE_USE_MATRIX)
-#define BGC_AFFINE_USE_MATRIX 1
+#define BGC_AFFINE_USE_MATRIX 0
 #endif
 
 inline int bgc_is_correct_axis(const int axis)