1 files changed, 52 insertions, 0 deletions
diff --git a/lib/jxl/enc_linalg.cc b/lib/jxl/enc_linalg.cc
new file mode 100644
index 0000000..fe2090a
--- /dev/null
+++ b/lib/jxl/enc_linalg.cc
@@ -0,0 +1,52 @@
+// Copyright (c) the JPEG XL Project Authors. All rights reserved.
+//
+// Use of this source code is governed by a BSD-style
+// license that can be found in the LICENSE file.
+
+#include "lib/jxl/enc_linalg.h"
+
+#include <cmath>
+
+#include "lib/jxl/base/compiler_specific.h"
+#include "lib/jxl/base/status.h"
+
+namespace jxl {
+
+void ConvertToDiagonal(const ImageD& A, ImageD* const JXL_RESTRICT diag,
+                       ImageD* const JXL_RESTRICT U) {
+#if JXL_ENABLE_ASSERT
+  JXL_ASSERT(A.xsize() == 2);
+  JXL_ASSERT(A.ysize() == 2);
+  JXL_ASSERT(std::abs(A.Row(0)[1] - A.Row(1)[0]) < 1e-15);
+#endif
+
+  if (std::abs(A.ConstRow(0)[1]) < 1e-15) {
+    // Already diagonal.
+    diag->Row(0)[0] = A.ConstRow(0)[0];
+    diag->Row(0)[1] = A.ConstRow(1)[1];
+    U->Row(0)[0] = U->Row(1)[1] = 1.0;
+    U->Row(0)[1] = U->Row(1)[0] = 0.0;
+    return;
+  }
+  double b = -(A.Row(0)[0] + A.Row(1)[1]);
+  double c = A.Row(0)[0] * A.Row(1)[1] - A.Row(0)[1] * A.Row(0)[1];
+  double d = b * b - 4.0 * c;
+  double sqd = std::sqrt(d);
+  double l1 = (-b - sqd) * 0.5;
+  double l2 = (-b + sqd) * 0.5;
+
+  double v1[2] = {A.Row(0)[0] - l1, A.Row(1)[0]};
+  double v1n = 1.0 / std::hypot(v1[0], v1[1]);
+  v1[0] = v1[0] * v1n;
+  v1[1] = v1[1] * v1n;
+
+  diag->Row(0)[0] = l1;
+  diag->Row(0)[1] = l2;
+
+  U->Row(0)[0] = v1[1];
+  U->Row(0)[1] = -v1[0];
+  U->Row(1)[0] = v1[0];
+  U->Row(1)[1] = v1[1];
+}
+
+}  // namespace jxl