Рефакторинг и оптимизация вычислений / Refactoring and optimization of computations

basic-geometry/matrix2x2.c

@@ -1 +1,105 @@
 #include "matrix2x2.h"
+
+// ================= Inversion ================== //
+
+int bg_fp32_matrix2x2_invert(BgFP32Matrix2x2* matrix)
+{
+    const float determinant = bg_fp32_matrix2x2_get_determinant(matrix);
+
+    if (-BG_FP32_EPSYLON <= determinant && determinant <= BG_FP32_EPSYLON) {
+        return 0;
+    }
+
+    const float r1c1 = matrix->r2c2;
+    const float r1c2 = -matrix->r1c2;
+
+    const float r2c1 = -matrix->r2c1;
+    const float r2c2 = matrix->r1c1;
+
+    const float multiplier = 1.0f / determinant;
+
+    matrix->r1c1 = r1c1 * multiplier;
+    matrix->r1c2 = r1c2 * multiplier;
+
+    matrix->r2c1 = r2c1 * multiplier;
+    matrix->r2c2 = r2c2 * multiplier;
+
+    return 1;
+}
+
+int bg_fp64_matrix2x2_invert(BgFP64Matrix2x2* matrix)
+{
+    const double determinant = bg_fp64_matrix2x2_get_determinant(matrix);
+
+    if (-BG_FP64_EPSYLON <= determinant && determinant <= BG_FP64_EPSYLON) {
+        return 0;
+    }
+
+    const double r1c1 = matrix->r2c2;
+    const double r1c2 = -matrix->r1c2;
+
+    const double r2c1 = -matrix->r2c1;
+    const double r2c2 = matrix->r1c1;
+
+    const double multiplier = 1.0 / determinant;
+
+    matrix->r1c1 = r1c1 * multiplier;
+    matrix->r1c2 = r1c2 * multiplier;
+
+    matrix->r2c1 = r2c1 * multiplier;
+    matrix->r2c2 = r2c2 * multiplier;
+
+    return 1;
+}
+
+// ================ Set Inverted ================ //
+
+int bg_fp32_matrix2x2_set_inverted(const BgFP32Matrix2x2* from, BgFP32Matrix2x2* to)
+{
+    const float determinant = bg_fp32_matrix2x2_get_determinant(from);
+
+    if (-BG_FP32_EPSYLON <= determinant && determinant <= BG_FP32_EPSYLON) {
+        return 0;
+    }
+
+    const float r1c1 = from->r2c2;
+    const float r1c2 = -from->r1c2;
+
+    const float r2c1 = -from->r2c1;
+    const float r2c2 = from->r1c1;
+
+    const float multiplier = 1.0f / determinant;
+
+    to->r1c1 = r1c1 * multiplier;
+    to->r1c2 = r1c2 * multiplier;
+
+    to->r2c1 = r2c1 * multiplier;
+    to->r2c2 = r2c2 * multiplier;
+
+    return 1;
+}
+
+int bg_fp64_matrix2x2_set_inverted(const BgFP64Matrix2x2* from, BgFP64Matrix2x2* to)
+{
+    const double determinant = bg_fp64_matrix2x2_get_determinant(from);
+
+    if (-BG_FP64_EPSYLON <= determinant && determinant <= BG_FP64_EPSYLON) {
+        return 0;
+    }
+
+    const double r1c1 = from->r2c2;
+    const double r1c2 = -from->r1c2;
+
+    const double r2c1 = -from->r2c1;
+    const double r2c2 = from->r1c1;
+
+    const double multiplier = 1.0 / determinant;
+
+    to->r1c1 = r1c1 * multiplier;
+    to->r1c2 = r1c2 * multiplier;
+
+    to->r2c1 = r2c1 * multiplier;
+    to->r2c2 = r2c2 * multiplier;
+
+    return 1;
+}