mathutils.h (4630B)
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 21 static const double TINY_NEAR_ZERO = 1.0E-16; 22 23 // Solves Ax = b, where x and b are column vectors of size nx1 and A is nxn 24 static inline int linsolve(int n, double *A, int stride, double *b, double *x) { 25 int i, j, k; 26 double c; 27 // Forward elimination 28 for (k = 0; k < n - 1; k++) { 29 // Bring the largest magnitude to the diagonal position 30 for (i = n - 1; i > k; i--) { 31 if (fabs(A[(i - 1) * stride + k]) < fabs(A[i * stride + k])) { 32 for (j = 0; j < n; j++) { 33 c = A[i * stride + j]; 34 A[i * stride + j] = A[(i - 1) * stride + j]; 35 A[(i - 1) * stride + j] = c; 36 } 37 c = b[i]; 38 b[i] = b[i - 1]; 39 b[i - 1] = c; 40 } 41 } 42 for (i = k; i < n - 1; i++) { 43 if (fabs(A[k * stride + k]) < TINY_NEAR_ZERO) return 0; 44 c = A[(i + 1) * stride + k] / A[k * stride + k]; 45 for (j = 0; j < n; j++) A[(i + 1) * stride + j] -= c * A[k * stride + j]; 46 b[i + 1] -= c * b[k]; 47 } 48 } 49 // Backward substitution 50 for (i = n - 1; i >= 0; i--) { 51 if (fabs(A[i * stride + i]) < TINY_NEAR_ZERO) return 0; 52 c = 0; 53 for (j = i + 1; j <= n - 1; j++) c += A[i * stride + j] * x[j]; 54 x[i] = (b[i] - c) / A[i * stride + i]; 55 } 56 57 return 1; 58 } 59 60 //////////////////////////////////////////////////////////////////////////////// 61 // Least-squares 62 // Solves for n-dim x in a least squares sense to minimize |Ax - b|^2 63 // The solution is simply x = (A'A)^-1 A'b or simply the solution for 64 // the system: A'A x = A'b 65 // 66 // This process is split into three steps in order to avoid needing to 67 // explicitly allocate the A matrix, which may be very large if there 68 // are many equations to solve. 69 // 70 // The process for using this is (in pseudocode): 71 // 72 // Allocate mat (size n*n), y (size n), a (size n), x (size n) 73 // least_squares_init(mat, y, n) 74 // for each equation a . x = b { 75 // least_squares_accumulate(mat, y, a, b, n) 76 // } 77 // least_squares_solve(mat, y, x, n) 78 // 79 // where: 80 // * mat, y are accumulators for the values A'A and A'b respectively, 81 // * a, b are the coefficients of each individual equation, 82 // * x is the result vector 83 // * and n is the problem size 84 static inline void least_squares_init(double *mat, double *y, int n) { 85 memset(mat, 0, n * n * sizeof(double)); 86 memset(y, 0, n * sizeof(double)); 87 } 88 89 // Round the given positive value to nearest integer 90 static AOM_FORCE_INLINE int iroundpf(float x) { 91 assert(x >= 0.0); 92 return (int)(x + 0.5f); 93 } 94 95 static inline void least_squares_accumulate(double *mat, double *y, 96 const double *a, double b, int n) { 97 for (int i = 0; i < n; i++) { 98 for (int j = 0; j < n; j++) { 99 mat[i * n + j] += a[i] * a[j]; 100 } 101 } 102 for (int i = 0; i < n; i++) { 103 y[i] += a[i] * b; 104 } 105 } 106 107 static inline int least_squares_solve(double *mat, double *y, double *x, 108 int n) { 109 return linsolve(n, mat, n, y, x); 110 } 111 112 // Matrix multiply 113 static inline void multiply_mat(const double *m1, const double *m2, double *res, 114 const int m1_rows, const int inner_dim, 115 const int m2_cols) { 116 double sum; 117 118 int row, col, inner; 119 for (row = 0; row < m1_rows; ++row) { 120 for (col = 0; col < m2_cols; ++col) { 121 sum = 0; 122 for (inner = 0; inner < inner_dim; ++inner) 123 sum += m1[row * inner_dim + inner] * m2[inner * m2_cols + col]; 124 *(res++) = sum; 125 } 126 } 127 } 128 129 static inline float approx_exp(float y) { 130 #define A ((1 << 23) / 0.69314718056f) // (1 << 23) / ln(2) 131 #define B \ 132 127 // Offset for the exponent according to IEEE floating point standard. 133 #define C 60801 // Magic number controls the accuracy of approximation 134 union { 135 float as_float; 136 int32_t as_int32; 137 } container; 138 container.as_int32 = ((int32_t)(y * A)) + ((B << 23) - C); 139 return container.as_float; 140 #undef A 141 #undef B 142 #undef C 143 } 144 #endif // AOM_AOM_DSP_MATHUTILS_H_