shared/matrix.c

   1 /*
   2  * Copyright © 2011 Intel Corporation
   3  * Copyright © 2012 Collabora, Ltd.
   4  *
   5  * Permission to use, copy, modify, distribute, and sell this software and
   6  * its documentation for any purpose is hereby granted without fee, provided
   7  * that the above copyright notice appear in all copies and that both that
   8  * copyright notice and this permission notice appear in supporting
   9  * documentation, and that the name of the copyright holders not be used in
  10  * advertising or publicity pertaining to distribution of the software
  11  * without specific, written prior permission.  The copyright holders make
  12  * no representations about the suitability of this software for any
  13  * purpose.  It is provided "as is" without express or implied warranty.
  14  *
  15  * THE COPYRIGHT HOLDERS DISCLAIM ALL WARRANTIES WITH REGARD TO THIS
  16  * SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND
  17  * FITNESS, IN NO EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE FOR ANY
  18  * SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER
  19  * RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF
  20  * CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
  21  * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  22  */
  23
  24 #include "config.h"
  25
  26 #include <float.h>
  27 #include <string.h>
  28 #include <stdlib.h>
  29 #include <math.h>
  30
  31 #ifdef IN_WESTON
  32 #include <wayland-server.h>
  33 #else
  34 #define WL_EXPORT
  35 #endif
  36
  37 #include "matrix.h"
  38
  39
  40 /*
  41  * Matrices are stored in column-major order, that is the array indices are:
  42  *  0  4  8 12
  43  *  1  5  9 13
  44  *  2  6 10 14
  45  *  3  7 11 15
  46  */
  47
  48 WL_EXPORT void
  49 weston_matrix_init(struct weston_matrix *matrix)
  50 {
  51         static const struct weston_matrix identity = {
  52                 .d = { 1, 0, 0, 0,  0, 1, 0, 0,  0, 0, 1, 0,  0, 0, 0, 1 },
  53                 .type = 0,
  54         };
  55
  56         memcpy(matrix, &identity, sizeof identity);
  57 }
  58
  59 /* m <- n * m, that is, m is multiplied on the LEFT. */
  60 WL_EXPORT void
  61 weston_matrix_multiply(struct weston_matrix *m, const struct weston_matrix *n)
  62 {
  63         struct weston_matrix tmp;
  64         const float *row, *column;
  65         div_t d;
  66         int i, j;
  67
  68         for (i = 0; i < 16; i++) {
  69                 tmp.d[i] = 0;
  70                 d = div(i, 4);
  71                 row = m->d + d.quot * 4;
  72                 column = n->d + d.rem;
  73                 for (j = 0; j < 4; j++)
  74                         tmp.d[i] += row[j] * column[j * 4];
  75         }
  76         tmp.type = m->type | n->type;
  77         memcpy(m, &tmp, sizeof tmp);
  78 }
  79
  80 WL_EXPORT void
  81 weston_matrix_translate(struct weston_matrix *matrix, float x, float y, float z)
  82 {
  83         struct weston_matrix translate = {
  84                 .d = { 1, 0, 0, 0,  0, 1, 0, 0,  0, 0, 1, 0,  x, y, z, 1 },
  85                 .type = WESTON_MATRIX_TRANSFORM_TRANSLATE,
  86         };
  87
  88         weston_matrix_multiply(matrix, &translate);
  89 }
  90
  91 WL_EXPORT void
  92 weston_matrix_scale(struct weston_matrix *matrix, float x, float y,float z)
  93 {
  94         struct weston_matrix scale = {
  95                 .d = { x, 0, 0, 0,  0, y, 0, 0,  0, 0, z, 0,  0, 0, 0, 1 },
  96                 .type = WESTON_MATRIX_TRANSFORM_SCALE,
  97         };
  98
  99         weston_matrix_multiply(matrix, &scale);
 100 }
 101
 102 WL_EXPORT void
 103 weston_matrix_rotate_xy(struct weston_matrix *matrix, float cos, float sin)
 104 {
 105         struct weston_matrix translate = {
 106                 .d = { cos, sin, 0, 0,  -sin, cos, 0, 0,  0, 0, 1, 0,  0, 0, 0, 1 },
 107                 .type = WESTON_MATRIX_TRANSFORM_ROTATE,
 108         };
 109
 110         weston_matrix_multiply(matrix, &translate);
 111 }
 112
 113 /* v <- m * v */
 114 WL_EXPORT void
 115 weston_matrix_transform(struct weston_matrix *matrix, struct weston_vector *v)
 116 {
 117         int i, j;
 118         struct weston_vector t;
 119
 120         for (i = 0; i < 4; i++) {
 121                 t.f[i] = 0;
 122                 for (j = 0; j < 4; j++)
 123                         t.f[i] += v->f[j] * matrix->d[i + j * 4];
 124         }
 125
 126         *v = t;
 127 }
 128
 129 static inline void
 130 swap_rows(double *a, double *b)
 131 {
 132         unsigned k;
 133         double tmp;
 134
 135         for (k = 0; k < 13; k += 4) {
 136                 tmp = a[k];
 137                 a[k] = b[k];
 138                 b[k] = tmp;
 139         }
 140 }
 141
 142 static inline void
 143 swap_unsigned(unsigned *a, unsigned *b)
 144 {
 145         unsigned tmp;
 146
 147         tmp = *a;
 148         *a = *b;
 149         *b = tmp;
 150 }
 151
 152 static inline unsigned
 153 find_pivot(double *column, unsigned k)
 154 {
 155         unsigned p = k;
 156         for (++k; k < 4; ++k)
 157                 if (fabs(column[p]) < fabs(column[k]))
 158                         p = k;
 159
 160         return p;
 161 }
 162
 163 /*
 164  * reference: Gene H. Golub and Charles F. van Loan. Matrix computations.
 165  * 3rd ed. The Johns Hopkins University Press. 1996.
 166  * LU decomposition, forward and back substitution: Chapter 3.
 167  */
 168
 169 MATRIX_TEST_EXPORT inline int
 170 matrix_invert(double *A, unsigned *p, const struct weston_matrix *matrix)
 171 {
 172         unsigned i, j, k;
 173         unsigned pivot;
 174         double pv;
 175
 176         for (i = 0; i < 4; ++i)
 177                 p[i] = i;
 178         for (i = 16; i--; )
 179                 A[i] = matrix->d[i];
 180
 181         /* LU decomposition with partial pivoting */
 182         for (k = 0; k < 4; ++k) {
 183                 pivot = find_pivot(&A[k * 4], k);
 184                 if (pivot != k) {
 185                         swap_unsigned(&p[k], &p[pivot]);
 186                         swap_rows(&A[k], &A[pivot]);
 187                 }
 188
 189                 pv = A[k * 4 + k];
 190                 if (fabs(pv) < 1e-9)
 191                         return -1; /* zero pivot, not invertible */
 192
 193                 for (i = k + 1; i < 4; ++i) {
 194                         A[i + k * 4] /= pv;
 195
 196                         for (j = k + 1; j < 4; ++j)
 197                                 A[i + j * 4] -= A[i + k * 4] * A[k + j * 4];
 198                 }
 199         }
 200
 201         return 0;
 202 }
 203
 204 MATRIX_TEST_EXPORT inline void
 205 inverse_transform(const double *LU, const unsigned *p, float *v)
 206 {
 207         /* Solve A * x = v, when we have P * A = L * U.
 208          * P * A * x = P * v  =>  L * U * x = P * v
 209          * Let U * x = b, then L * b = P * v.
 210          */
 211         double b[4];
 212         unsigned j;
 213
 214         /* Forward substitution, column version, solves L * b = P * v */
 215         /* The diagonal of L is all ones, and not explicitly stored. */
 216         b[0] = v[p[0]];
 217         b[1] = (double)v[p[1]] - b[0] * LU[1 + 0 * 4];
 218         b[2] = (double)v[p[2]] - b[0] * LU[2 + 0 * 4];
 219         b[3] = (double)v[p[3]] - b[0] * LU[3 + 0 * 4];
 220         b[2] -= b[1] * LU[2 + 1 * 4];
 221         b[3] -= b[1] * LU[3 + 1 * 4];
 222         b[3] -= b[2] * LU[3 + 2 * 4];
 223
 224         /* backward substitution, column version, solves U * y = b */
 225 #if 1
 226         /* hand-unrolled, 25% faster for whole function */
 227         b[3] /= LU[3 + 3 * 4];
 228         b[0] -= b[3] * LU[0 + 3 * 4];
 229         b[1] -= b[3] * LU[1 + 3 * 4];
 230         b[2] -= b[3] * LU[2 + 3 * 4];
 231
 232         b[2] /= LU[2 + 2 * 4];
 233         b[0] -= b[2] * LU[0 + 2 * 4];
 234         b[1] -= b[2] * LU[1 + 2 * 4];
 235
 236         b[1] /= LU[1 + 1 * 4];
 237         b[0] -= b[1] * LU[0 + 1 * 4];
 238
 239         b[0] /= LU[0 + 0 * 4];
 240 #else
 241         for (j = 3; j > 0; --j) {
 242                 unsigned k;
 243                 b[j] /= LU[j + j * 4];
 244                 for (k = 0; k < j; ++k)
 245                         b[k] -= b[j] * LU[k + j * 4];
 246         }
 247
 248         b[0] /= LU[0 + 0 * 4];
 249 #endif
 250
 251         /* the result */
 252         for (j = 0; j < 4; ++j)
 253                 v[j] = b[j];
 254 }
 255
 256 WL_EXPORT int
 257 weston_matrix_invert(struct weston_matrix *inverse,
 258                      const struct weston_matrix *matrix)
 259 {
 260         double LU[16];          /* column-major */
 261         unsigned perm[4];       /* permutation */
 262         unsigned c;
 263
 264         if (matrix_invert(LU, perm, matrix) < 0)
 265                 return -1;
 266
 267         weston_matrix_init(inverse);
 268         for (c = 0; c < 4; ++c)
 269                 inverse_transform(LU, perm, &inverse->d[c * 4]);
 270         inverse->type = matrix->type;
 271
 272         return 0;
 273 }