| 1 | /* Copyright (C) 2010-2020 The RetroArch team |
| 2 | * |
| 3 | * --------------------------------------------------------------------------------------- |
| 4 | * The following license statement only applies to this file (matrix_3x3.h). |
| 5 | * --------------------------------------------------------------------------------------- |
| 6 | * |
| 7 | * Permission is hereby granted, free of charge, |
| 8 | * to any person obtaining a copy of this software and associated documentation files (the "Software"), |
| 9 | * to deal in the Software without restriction, including without limitation the rights to |
| 10 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, |
| 11 | * and to permit persons to whom the Software is furnished to do so, subject to the following conditions: |
| 12 | * |
| 13 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. |
| 14 | * |
| 15 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, |
| 16 | * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
| 17 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. |
| 18 | * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, |
| 19 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
| 20 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. |
| 21 | */ |
| 22 | |
| 23 | #ifndef __LIBRETRO_SDK_GFX_MATH_MATRIX_3X3_H__ |
| 24 | #define __LIBRETRO_SDK_GFX_MATH_MATRIX_3X3_H__ |
| 25 | |
| 26 | #include <boolean.h> |
| 27 | #include <math.h> |
| 28 | #include <string.h> |
| 29 | |
| 30 | #include <retro_common_api.h> |
| 31 | #include <retro_inline.h> |
| 32 | |
| 33 | RETRO_BEGIN_DECLS |
| 34 | |
| 35 | typedef struct math_matrix_3x3 |
| 36 | { |
| 37 | float data[9]; |
| 38 | } math_matrix_3x3; |
| 39 | |
| 40 | #define MAT_ELEM_3X3(mat, r, c) ((mat).data[3 * (r) + (c)]) |
| 41 | |
| 42 | #define matrix_3x3_init(mat, n11, n12, n13, n21, n22, n23, n31, n32, n33) \ |
| 43 | MAT_ELEM_3X3(mat, 0, 0) = n11; \ |
| 44 | MAT_ELEM_3X3(mat, 0, 1) = n12; \ |
| 45 | MAT_ELEM_3X3(mat, 0, 2) = n13; \ |
| 46 | MAT_ELEM_3X3(mat, 1, 0) = n21; \ |
| 47 | MAT_ELEM_3X3(mat, 1, 1) = n22; \ |
| 48 | MAT_ELEM_3X3(mat, 1, 2) = n23; \ |
| 49 | MAT_ELEM_3X3(mat, 2, 0) = n31; \ |
| 50 | MAT_ELEM_3X3(mat, 2, 1) = n32; \ |
| 51 | MAT_ELEM_3X3(mat, 2, 2) = n33 |
| 52 | |
| 53 | #define matrix_3x3_identity(mat) \ |
| 54 | MAT_ELEM_3X3(mat, 0, 0) = 1.0f; \ |
| 55 | MAT_ELEM_3X3(mat, 0, 1) = 0; \ |
| 56 | MAT_ELEM_3X3(mat, 0, 2) = 0; \ |
| 57 | MAT_ELEM_3X3(mat, 1, 0) = 0; \ |
| 58 | MAT_ELEM_3X3(mat, 1, 1) = 1.0f; \ |
| 59 | MAT_ELEM_3X3(mat, 1, 2) = 0; \ |
| 60 | MAT_ELEM_3X3(mat, 2, 0) = 0; \ |
| 61 | MAT_ELEM_3X3(mat, 2, 1) = 0; \ |
| 62 | MAT_ELEM_3X3(mat, 2, 2) = 1.0f |
| 63 | |
| 64 | #define matrix_3x3_divide_scalar(mat, s) \ |
| 65 | MAT_ELEM_3X3(mat, 0, 0) /= s; \ |
| 66 | MAT_ELEM_3X3(mat, 0, 1) /= s; \ |
| 67 | MAT_ELEM_3X3(mat, 0, 2) /= s; \ |
| 68 | MAT_ELEM_3X3(mat, 1, 0) /= s; \ |
| 69 | MAT_ELEM_3X3(mat, 1, 1) /= s; \ |
| 70 | MAT_ELEM_3X3(mat, 1, 2) /= s; \ |
| 71 | MAT_ELEM_3X3(mat, 2, 0) /= s; \ |
| 72 | MAT_ELEM_3X3(mat, 2, 1) /= s; \ |
| 73 | MAT_ELEM_3X3(mat, 2, 2) /= s |
| 74 | |
| 75 | #define matrix_3x3_transpose(mat, in) \ |
| 76 | MAT_ELEM_3X3(mat, 0, 0) = MAT_ELEM_3X3(in, 0, 0); \ |
| 77 | MAT_ELEM_3X3(mat, 1, 0) = MAT_ELEM_3X3(in, 0, 1); \ |
| 78 | MAT_ELEM_3X3(mat, 2, 0) = MAT_ELEM_3X3(in, 0, 2); \ |
| 79 | MAT_ELEM_3X3(mat, 0, 1) = MAT_ELEM_3X3(in, 1, 0); \ |
| 80 | MAT_ELEM_3X3(mat, 1, 1) = MAT_ELEM_3X3(in, 1, 1); \ |
| 81 | MAT_ELEM_3X3(mat, 2, 1) = MAT_ELEM_3X3(in, 1, 2); \ |
| 82 | MAT_ELEM_3X3(mat, 0, 2) = MAT_ELEM_3X3(in, 2, 0); \ |
| 83 | MAT_ELEM_3X3(mat, 1, 2) = MAT_ELEM_3X3(in, 2, 1); \ |
| 84 | MAT_ELEM_3X3(mat, 2, 2) = MAT_ELEM_3X3(in, 2, 2) |
| 85 | |
| 86 | #define matrix_3x3_multiply(out, a, b) \ |
| 87 | MAT_ELEM_3X3(out, 0, 0) = \ |
| 88 | MAT_ELEM_3X3(a, 0, 0) * MAT_ELEM_3X3(b, 0, 0) + \ |
| 89 | MAT_ELEM_3X3(a, 0, 1) * MAT_ELEM_3X3(b, 1, 0) + \ |
| 90 | MAT_ELEM_3X3(a, 0, 2) * MAT_ELEM_3X3(b, 2, 0); \ |
| 91 | MAT_ELEM_3X3(out, 0, 1) = \ |
| 92 | MAT_ELEM_3X3(a, 0, 0) * MAT_ELEM_3X3(b, 0, 1) + \ |
| 93 | MAT_ELEM_3X3(a, 0, 1) * MAT_ELEM_3X3(b, 1, 1) + \ |
| 94 | MAT_ELEM_3X3(a, 0, 2) * MAT_ELEM_3X3(b, 2, 1); \ |
| 95 | MAT_ELEM_3X3(out, 0, 2) = \ |
| 96 | MAT_ELEM_3X3(a, 0, 0) * MAT_ELEM_3X3(b, 0, 2) + \ |
| 97 | MAT_ELEM_3X3(a, 0, 1) * MAT_ELEM_3X3(b, 1, 2) + \ |
| 98 | MAT_ELEM_3X3(a, 0, 2) * MAT_ELEM_3X3(b, 2, 2); \ |
| 99 | MAT_ELEM_3X3(out, 1, 0) = \ |
| 100 | MAT_ELEM_3X3(a, 1, 0) * MAT_ELEM_3X3(b, 0, 0) + \ |
| 101 | MAT_ELEM_3X3(a, 1, 1) * MAT_ELEM_3X3(b, 1, 0) + \ |
| 102 | MAT_ELEM_3X3(a, 1, 2) * MAT_ELEM_3X3(b, 2, 0); \ |
| 103 | MAT_ELEM_3X3(out, 1, 1) = \ |
| 104 | MAT_ELEM_3X3(a, 1, 0) * MAT_ELEM_3X3(b, 0, 1) + \ |
| 105 | MAT_ELEM_3X3(a, 1, 1) * MAT_ELEM_3X3(b, 1, 1) + \ |
| 106 | MAT_ELEM_3X3(a, 1, 2) * MAT_ELEM_3X3(b, 2, 1); \ |
| 107 | MAT_ELEM_3X3(out, 1, 2) = \ |
| 108 | MAT_ELEM_3X3(a, 1, 0) * MAT_ELEM_3X3(b, 0, 2) + \ |
| 109 | MAT_ELEM_3X3(a, 1, 1) * MAT_ELEM_3X3(b, 1, 2) + \ |
| 110 | MAT_ELEM_3X3(a, 1, 2) * MAT_ELEM_3X3(b, 2, 2); \ |
| 111 | MAT_ELEM_3X3(out, 2, 0) = \ |
| 112 | MAT_ELEM_3X3(a, 2, 0) * MAT_ELEM_3X3(b, 0, 0) + \ |
| 113 | MAT_ELEM_3X3(a, 2, 1) * MAT_ELEM_3X3(b, 1, 0) + \ |
| 114 | MAT_ELEM_3X3(a, 2, 2) * MAT_ELEM_3X3(b, 2, 0); \ |
| 115 | MAT_ELEM_3X3(out, 2, 1) = \ |
| 116 | MAT_ELEM_3X3(a, 2, 0) * MAT_ELEM_3X3(b, 0, 1) + \ |
| 117 | MAT_ELEM_3X3(a, 2, 1) * MAT_ELEM_3X3(b, 1, 1) + \ |
| 118 | MAT_ELEM_3X3(a, 2, 2) * MAT_ELEM_3X3(b, 2, 1); \ |
| 119 | MAT_ELEM_3X3(out, 2, 2) = \ |
| 120 | MAT_ELEM_3X3(a, 2, 0) * MAT_ELEM_3X3(b, 0, 2) + \ |
| 121 | MAT_ELEM_3X3(a, 2, 1) * MAT_ELEM_3X3(b, 1, 2) + \ |
| 122 | MAT_ELEM_3X3(a, 2, 2) * MAT_ELEM_3X3(b, 2, 2) |
| 123 | |
| 124 | #define matrix_3x3_determinant(mat) (MAT_ELEM_3X3(mat, 0, 0) * (MAT_ELEM_3X3(mat, 1, 1) * MAT_ELEM_3X3(mat, 2, 2) - MAT_ELEM_3X3(mat, 1, 2) * MAT_ELEM_3X3(mat, 2, 1)) - MAT_ELEM_3X3(mat, 0, 1) * (MAT_ELEM_3X3(mat, 1, 0) * MAT_ELEM_3X3(mat, 2, 2) - MAT_ELEM_3X3(mat, 1, 2) * MAT_ELEM_3X3(mat, 2, 0)) + MAT_ELEM_3X3(mat, 0, 2) * (MAT_ELEM_3X3(mat, 1, 0) * MAT_ELEM_3X3(mat, 2, 1) - MAT_ELEM_3X3(mat, 1, 1) * MAT_ELEM_3X3(mat, 2, 0))) |
| 125 | |
| 126 | #define matrix_3x3_adjoint(mat) \ |
| 127 | MAT_ELEM_3X3(mat, 0, 0) = (MAT_ELEM_3X3(mat, 1, 1) * MAT_ELEM_3X3(mat, 2, 2) - MAT_ELEM_3X3(mat, 1, 2) * MAT_ELEM_3X3(mat, 2, 1)); \ |
| 128 | MAT_ELEM_3X3(mat, 0, 1) = -(MAT_ELEM_3X3(mat, 0, 1) * MAT_ELEM_3X3(mat, 2, 2) - MAT_ELEM_3X3(mat, 0, 2) * MAT_ELEM_3X3(mat, 2, 1)); \ |
| 129 | MAT_ELEM_3X3(mat, 0, 2) = (MAT_ELEM_3X3(mat, 0, 1) * MAT_ELEM_3X3(mat, 1, 1) - MAT_ELEM_3X3(mat, 0, 2) * MAT_ELEM_3X3(mat, 1, 1)); \ |
| 130 | MAT_ELEM_3X3(mat, 1, 0) = -(MAT_ELEM_3X3(mat, 1, 0) * MAT_ELEM_3X3(mat, 2, 2) - MAT_ELEM_3X3(mat, 1, 2) * MAT_ELEM_3X3(mat, 2, 0)); \ |
| 131 | MAT_ELEM_3X3(mat, 1, 1) = (MAT_ELEM_3X3(mat, 0, 0) * MAT_ELEM_3X3(mat, 2, 2) - MAT_ELEM_3X3(mat, 0, 2) * MAT_ELEM_3X3(mat, 2, 0)); \ |
| 132 | MAT_ELEM_3X3(mat, 1, 2) = -(MAT_ELEM_3X3(mat, 0, 0) * MAT_ELEM_3X3(mat, 1, 2) - MAT_ELEM_3X3(mat, 0, 2) * MAT_ELEM_3X3(mat, 1, 0)); \ |
| 133 | MAT_ELEM_3X3(mat, 2, 0) = (MAT_ELEM_3X3(mat, 1, 0) * MAT_ELEM_3X3(mat, 2, 1) - MAT_ELEM_3X3(mat, 1, 1) * MAT_ELEM_3X3(mat, 2, 0)); \ |
| 134 | MAT_ELEM_3X3(mat, 2, 1) = -(MAT_ELEM_3X3(mat, 0, 0) * MAT_ELEM_3X3(mat, 2, 1) - MAT_ELEM_3X3(mat, 0, 1) * MAT_ELEM_3X3(mat, 2, 0)); \ |
| 135 | MAT_ELEM_3X3(mat, 2, 2) = (MAT_ELEM_3X3(mat, 0, 0) * MAT_ELEM_3X3(mat, 1, 1) - MAT_ELEM_3X3(mat, 0, 1) * MAT_ELEM_3X3(mat, 1, 0)) |
| 136 | |
| 137 | #define FLOATS_ARE_EQUAL(x, y) (fabs(x - y) <= 0.00001f * ((x) > (y) ? (y) : (x))) |
| 138 | #define FLOAT_IS_ZERO(x) (FLOATS_ARE_EQUAL((x) + 1, 1)) |
| 139 | |
| 140 | static INLINE bool matrix_3x3_invert(math_matrix_3x3 *mat) |
| 141 | { |
| 142 | float det = matrix_3x3_determinant(*mat); |
| 143 | |
| 144 | if (FLOAT_IS_ZERO(det)) |
| 145 | return false; |
| 146 | |
| 147 | matrix_3x3_adjoint(*mat); |
| 148 | matrix_3x3_divide_scalar(*mat, det); |
| 149 | |
| 150 | return true; |
| 151 | } |
| 152 | |
| 153 | static INLINE bool matrix_3x3_square_to_quad( |
| 154 | const float dx0, const float dy0, |
| 155 | const float dx1, const float dy1, |
| 156 | const float dx3, const float dy3, |
| 157 | const float dx2, const float dy2, |
| 158 | math_matrix_3x3 *mat) |
| 159 | { |
| 160 | float a, b, d, e; |
| 161 | float ax = dx0 - dx1 + dx2 - dx3; |
| 162 | float ay = dy0 - dy1 + dy2 - dy3; |
| 163 | float c = dx0; |
| 164 | float f = dy0; |
| 165 | float g = 0; |
| 166 | float h = 0; |
| 167 | |
| 168 | if (FLOAT_IS_ZERO(ax) && FLOAT_IS_ZERO(ay)) |
| 169 | { |
| 170 | /* affine case */ |
| 171 | a = dx1 - dx0; |
| 172 | b = dx2 - dx1; |
| 173 | d = dy1 - dy0; |
| 174 | e = dy2 - dy1; |
| 175 | } |
| 176 | else |
| 177 | { |
| 178 | float ax1 = dx1 - dx2; |
| 179 | float ax2 = dx3 - dx2; |
| 180 | float ay1 = dy1 - dy2; |
| 181 | float ay2 = dy3 - dy2; |
| 182 | |
| 183 | /* determinants */ |
| 184 | float gtop = ax * ay2 - ax2 * ay; |
| 185 | float htop = ax1 * ay - ax * ay1; |
| 186 | float bottom = ax1 * ay2 - ax2 * ay1; |
| 187 | |
| 188 | if (!bottom) |
| 189 | return false; |
| 190 | |
| 191 | g = gtop / bottom; |
| 192 | h = htop / bottom; |
| 193 | |
| 194 | a = dx1 - dx0 + g * dx1; |
| 195 | b = dx3 - dx0 + h * dx3; |
| 196 | d = dy1 - dy0 + g * dy1; |
| 197 | e = dy3 - dy0 + h * dy3; |
| 198 | } |
| 199 | |
| 200 | matrix_3x3_init(*mat, |
| 201 | a, d, g, |
| 202 | b, e, h, |
| 203 | c, f, 1.f); |
| 204 | |
| 205 | return true; |
| 206 | } |
| 207 | |
| 208 | static INLINE bool matrix_3x3_quad_to_square( |
| 209 | const float sx0, const float sy0, |
| 210 | const float sx1, const float sy1, |
| 211 | const float sx2, const float sy2, |
| 212 | const float sx3, const float sy3, |
| 213 | math_matrix_3x3 *mat) |
| 214 | { |
| 215 | return matrix_3x3_square_to_quad(sx0, sy0, sx1, sy1, |
| 216 | sx2, sy2, sx3, sy3, |
| 217 | mat) ? matrix_3x3_invert(mat) : false; |
| 218 | } |
| 219 | |
| 220 | static INLINE bool matrix_3x3_quad_to_quad( |
| 221 | const float dx0, const float dy0, |
| 222 | const float dx1, const float dy1, |
| 223 | const float dx2, const float dy2, |
| 224 | const float dx3, const float dy3, |
| 225 | const float sx0, const float sy0, |
| 226 | const float sx1, const float sy1, |
| 227 | const float sx2, const float sy2, |
| 228 | const float sx3, const float sy3, |
| 229 | math_matrix_3x3 *mat) |
| 230 | { |
| 231 | math_matrix_3x3 square_to_quad; |
| 232 | |
| 233 | if (matrix_3x3_square_to_quad(dx0, dy0, dx1, dy1, |
| 234 | dx2, dy2, dx3, dy3, |
| 235 | &square_to_quad)) |
| 236 | { |
| 237 | math_matrix_3x3 quad_to_square; |
| 238 | if (matrix_3x3_quad_to_square(sx0, sy0, sx1, sy1, |
| 239 | sx2, sy2, sx3, sy3, |
| 240 | &quad_to_square)) |
| 241 | { |
| 242 | matrix_3x3_multiply(*mat, quad_to_square, square_to_quad); |
| 243 | |
| 244 | return true; |
| 245 | } |
| 246 | } |
| 247 | |
| 248 | return false; |
| 249 | } |
| 250 | |
| 251 | RETRO_END_DECLS |
| 252 | |
| 253 | #endif |