1 /*
2 * Copyright (c) 2017, Alliance for Open Media. All rights reserved
3 *
4 * This source code is subject to the terms of the BSD 2 Clause License and
5 * the Alliance for Open Media Patent License 1.0. If the BSD 2 Clause License
6 * was not distributed with this source code in the LICENSE file, you can
7 * obtain it at www.aomedia.org/license/software. If the Alliance for Open
8 * Media Patent License 1.0 was not distributed with this source code in the
9 * PATENTS file, you can obtain it at www.aomedia.org/license/patent.
10 */
11
12 #ifndef AOM_AOM_DSP_MATHUTILS_H_
13 #define AOM_AOM_DSP_MATHUTILS_H_
14
15 #include <assert.h>
16 #include <math.h>
17 #include <string.h>
18
19 #include "aom_dsp/aom_dsp_common.h"
20 #include "aom_mem/aom_mem.h"
21
22 static const double TINY_NEAR_ZERO = 1.0E-16;
23
24 // Solves Ax = b, where x and b are column vectors of size nx1 and A is nxn
linsolve(int n,double * A,int stride,double * b,double * x)25 static INLINE int linsolve(int n, double *A, int stride, double *b, double *x) {
26 int i, j, k;
27 double c;
28 // Forward elimination
29 for (k = 0; k < n - 1; k++) {
30 // Bring the largest magnitude to the diagonal position
31 for (i = n - 1; i > k; i--) {
32 if (fabs(A[(i - 1) * stride + k]) < fabs(A[i * stride + k])) {
33 for (j = 0; j < n; j++) {
34 c = A[i * stride + j];
35 A[i * stride + j] = A[(i - 1) * stride + j];
36 A[(i - 1) * stride + j] = c;
37 }
38 c = b[i];
39 b[i] = b[i - 1];
40 b[i - 1] = c;
41 }
42 }
43 for (i = k; i < n - 1; i++) {
44 if (fabs(A[k * stride + k]) < TINY_NEAR_ZERO) return 0;
45 c = A[(i + 1) * stride + k] / A[k * stride + k];
46 for (j = 0; j < n; j++) A[(i + 1) * stride + j] -= c * A[k * stride + j];
47 b[i + 1] -= c * b[k];
48 }
49 }
50 // Backward substitution
51 for (i = n - 1; i >= 0; i--) {
52 if (fabs(A[i * stride + i]) < TINY_NEAR_ZERO) return 0;
53 c = 0;
54 for (j = i + 1; j <= n - 1; j++) c += A[i * stride + j] * x[j];
55 x[i] = (b[i] - c) / A[i * stride + i];
56 }
57
58 return 1;
59 }
60
61 ////////////////////////////////////////////////////////////////////////////////
62 // Least-squares
63 // Solves for n-dim x in a least squares sense to minimize |Ax - b|^2
64 // The solution is simply x = (A'A)^-1 A'b or simply the solution for
65 // the system: A'A x = A'b
least_squares(int n,double * A,int rows,int stride,double * b,double * scratch,double * x)66 static INLINE int least_squares(int n, double *A, int rows, int stride,
67 double *b, double *scratch, double *x) {
68 int i, j, k;
69 double *scratch_ = NULL;
70 double *AtA, *Atb;
71 if (!scratch) {
72 scratch_ = (double *)aom_malloc(sizeof(*scratch) * n * (n + 1));
73 if (!scratch_) return 0;
74 scratch = scratch_;
75 }
76 AtA = scratch;
77 Atb = scratch + n * n;
78
79 for (i = 0; i < n; ++i) {
80 for (j = i; j < n; ++j) {
81 AtA[i * n + j] = 0.0;
82 for (k = 0; k < rows; ++k)
83 AtA[i * n + j] += A[k * stride + i] * A[k * stride + j];
84 AtA[j * n + i] = AtA[i * n + j];
85 }
86 Atb[i] = 0;
87 for (k = 0; k < rows; ++k) Atb[i] += A[k * stride + i] * b[k];
88 }
89 int ret = linsolve(n, AtA, n, Atb, x);
90 aom_free(scratch_);
91 return ret;
92 }
93
94 // Matrix multiply
multiply_mat(const double * m1,const double * m2,double * res,const int m1_rows,const int inner_dim,const int m2_cols)95 static INLINE void multiply_mat(const double *m1, const double *m2, double *res,
96 const int m1_rows, const int inner_dim,
97 const int m2_cols) {
98 double sum;
99
100 int row, col, inner;
101 for (row = 0; row < m1_rows; ++row) {
102 for (col = 0; col < m2_cols; ++col) {
103 sum = 0;
104 for (inner = 0; inner < inner_dim; ++inner)
105 sum += m1[row * inner_dim + inner] * m2[inner * m2_cols + col];
106 *(res++) = sum;
107 }
108 }
109 }
110
111 #endif // AOM_AOM_DSP_MATHUTILS_H_
112