deps/libretro-common/include/gfx/math/matrix_3x3.h

   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