gts/matrix.c

   1 /* GTS - Library for the manipulation of triangulated surfaces
   2  * Copyright (C) 1999 Stéphane Popinet
   3  *
   4  * This library is free software; you can redistribute it and/or
   5  * modify it under the terms of the GNU Library General Public
   6  * License as published by the Free Software Foundation; either
   7  * version 2 of the License, or (at your option) any later version.
   8  *
   9  * This library is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12  * Library General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU Library General Public
  15  * License along with this library; if not, write to the
  16  * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
  17  * Boston, MA 02111-1307, USA.
  18  */
  19
  20 #include <math.h>
  21 #include "gts.h"
  22
  23 /**
  24  * gts_matrix_new:
  25  * @a00: element [0][0].
  26  * @a01: element [0][1].
  27  * @a02: element [0][2].
  28  * @a03: element [0][3].
  29  * @a10: element [1][0].
  30  * @a11: element [1][1].
  31  * @a12: element [1][2].
  32  * @a13: element [1][3].
  33  * @a20: element [2][0].
  34  * @a21: element [2][1].
  35  * @a22: element [2][2].
  36  * @a23: element [2][3].
  37  * @a30: element [3][0].
  38  * @a31: element [3][1].
  39  * @a32: element [3][2].
  40  * @a33: element [3][3].
  41  *
  42  * Allocates memory and initializes a new #GtsMatrix.
  43  *
  44  * Returns: a pointer to the newly created #GtsMatrix.
  45  */
  46 GtsMatrix * gts_matrix_new (gdouble a00, gdouble a01, gdouble a02, gdouble a03,
  47                             gdouble a10, gdouble a11, gdouble a12, gdouble a13,
  48                             gdouble a20, gdouble a21, gdouble a22, gdouble a23,
  49                             gdouble a30, gdouble a31, gdouble a32, gdouble a33)
  50 {
  51   GtsMatrix * m;
  52
  53   m = g_malloc (4*sizeof (GtsVector4));
  54
  55   m[0][0] = a00; m[1][0] = a10; m[2][0] = a20; m[3][0] = a30;
  56   m[0][1] = a01; m[1][1] = a11; m[2][1] = a21; m[3][1] = a31;
  57   m[0][2] = a02; m[1][2] = a12; m[2][2] = a22; m[3][2] = a32;
  58   m[0][3] = a03; m[1][3] = a13; m[2][3] = a23; m[3][3] = a33;
  59
  60   return m;
  61 }
  62
  63 /**
  64  * gts_matrix_assign:
  65  * @m: a #GtsMatrix.
  66  * @a00: element [0][0].
  67  * @a01: element [0][1].
  68  * @a02: element [0][2].
  69  * @a03: element [0][3].
  70  * @a10: element [1][0].
  71  * @a11: element [1][1].
  72  * @a12: element [1][2].
  73  * @a13: element [1][3].
  74  * @a20: element [2][0].
  75  * @a21: element [2][1].
  76  * @a22: element [2][2].
  77  * @a23: element [2][3].
  78  * @a30: element [3][0].
  79  * @a31: element [3][1].
  80  * @a32: element [3][2].
  81  * @a33: element [3][3].
  82  *
  83  * Set values of matrix elements.
  84  */
  85 void gts_matrix_assign (GtsMatrix * m,
  86                         gdouble a00, gdouble a01, gdouble a02, gdouble a03,
  87                         gdouble a10, gdouble a11, gdouble a12, gdouble a13,
  88                         gdouble a20, gdouble a21, gdouble a22, gdouble a23,
  89                         gdouble a30, gdouble a31, gdouble a32, gdouble a33)
  90 {
  91   g_return_if_fail (m != NULL);
  92
  93   m[0][0] = a00; m[1][0] = a10; m[2][0] = a20; m[3][0] = a30;
  94   m[0][1] = a01; m[1][1] = a11; m[2][1] = a21; m[3][1] = a31;
  95   m[0][2] = a02; m[1][2] = a12; m[2][2] = a22; m[3][2] = a32;
  96   m[0][3] = a03; m[1][3] = a13; m[2][3] = a23; m[3][3] = a33;
  97 }
  98
  99 /**
 100  * gts_matrix_projection:
 101  * @t: a #GtsTriangle.
 102  *
 103  * Creates a new #GtsMatrix representing the projection onto a plane of normal
 104  * given by @t.
 105  *
 106  * Returns: a pointer to the newly created #GtsMatrix.
 107  */
 108 GtsMatrix * gts_matrix_projection (GtsTriangle * t)
 109 {
 110   GtsVertex * v1, * v2, * v3;
 111   GtsEdge * e1, * e2, * e3;
 112   GtsMatrix * m;
 113   gdouble x1, y1, z1, x2, y2, z2, x3, y3, z3, l;
 114
 115   g_return_val_if_fail (t != NULL, NULL);
 116
 117   m = g_malloc (4*sizeof (GtsVector4));
 118   gts_triangle_vertices_edges (t, NULL, &v1, &v2, &v3, &e1, &e2, &e3);
 119
 120   x1 = GTS_POINT (v2)->x - GTS_POINT (v1)->x;
 121   y1 = GTS_POINT (v2)->y - GTS_POINT (v1)->y;
 122   z1 = GTS_POINT (v2)->z - GTS_POINT (v1)->z;
 123   x2 = GTS_POINT (v3)->x - GTS_POINT (v1)->x;
 124   y2 = GTS_POINT (v3)->y - GTS_POINT (v1)->y;
 125   z2 = GTS_POINT (v3)->z - GTS_POINT (v1)->z;
 126   x3 = y1*z2 - z1*y2; y3 = z1*x2 - x1*z2; z3 = x1*y2 - y1*x2;
 127   x2 = y3*z1 - z3*y1; y2 = z3*x1 - x3*z1; z2 = x3*y1 - y3*x1;
 128
 129   l = sqrt (x1*x1 + y1*y1 + z1*z1);
 130   g_assert (l > 0.0);
 131   m[0][0] = x1/l; m[1][0] = y1/l; m[2][0] = z1/l; m[3][0] = 0.;
 132   l = sqrt (x2*x2 + y2*y2 + z2*z2);
 133   g_assert (l > 0.0);
 134   m[0][1] = x2/l; m[1][1] = y2/l; m[2][1] = z2/l; m[3][1] = 0.;
 135   l = sqrt (x3*x3 + y3*y3 + z3*z3);
 136   g_assert (l > 0.0);
 137   m[0][2] = x3/l; m[1][2] = y3/l; m[2][2] = z3/l; m[3][2] = 0.;
 138   m[0][3] = 0; m[1][3] = 0.; m[2][3] = 0.; m[3][3] = 1.;
 139
 140   return m;
 141 }
 142
 143 /**
 144  * gts_matrix_transpose:
 145  * @m: a #GtsMatrix.
 146  *
 147  * Returns: a pointer to a newly created #GtsMatrix transposed of @m.
 148  */
 149 GtsMatrix * gts_matrix_transpose (GtsMatrix * m)
 150 {
 151   GtsMatrix * mi;
 152
 153   g_return_val_if_fail (m != NULL, NULL);
 154
 155   mi = g_malloc (4*sizeof (GtsVector4));
 156
 157   mi[0][0] = m[0][0]; mi[1][0] = m[0][1];
 158   mi[2][0] = m[0][2]; mi[3][0] = m[0][3];
 159   mi[0][1] = m[1][0]; mi[1][1] = m[1][1];
 160   mi[2][1] = m[1][2]; mi[3][1] = m[1][3];
 161   mi[0][2] = m[2][0]; mi[1][2] = m[2][1];
 162   mi[2][2] = m[2][2]; mi[3][2] = m[2][3];
 163   mi[0][3] = m[3][0]; mi[1][3] = m[3][1];
 164   mi[2][3] = m[3][2]; mi[3][3] = m[3][3];
 165
 166   return mi;
 167 }
 168
 169 /*
 170  * calculate the determinant of a 2x2 matrix.
 171  *
 172  * Adapted from:
 173  * Matrix Inversion
 174  * by Richard Carling
 175  * from "Graphics Gems", Academic Press, 1990
 176  */
 177 static gdouble det2x2 (gdouble a, gdouble b, gdouble c, gdouble d)
 178 {
 179   gdouble ans2;
 180
 181   ans2 = a*d - b*c;
 182   return ans2;
 183 }
 184
 185 /*
 186  * calculate the determinant of a 3x3 matrix
 187  * in the form
 188  *
 189  *     | a1,  b1,  c1 |
 190  *     | a2,  b2,  c2 |
 191  *     | a3,  b3,  c3 |
 192  *
 193  * Adapted from:
 194  * Matrix Inversion
 195  * by Richard Carling
 196  * from "Graphics Gems", Academic Press, 1990
 197  */
 198 static gdouble det3x3 (gdouble a1, gdouble a2, gdouble a3,
 199                        gdouble b1, gdouble b2, gdouble b3,
 200                        gdouble c1, gdouble c2, gdouble c3)
 201 {
 202   gdouble ans3;
 203
 204   ans3 = a1 * det2x2( b2, b3, c2, c3 )
 205     - b1 * det2x2( a2, a3, c2, c3 )
 206     + c1 * det2x2( a2, a3, b2, b3 );
 207   return ans3;
 208 }
 209
 210 /**
 211  * gts_matrix_determinant:
 212  * @m: a #GtsMatrix.
 213  *
 214  * Returns: the value of det(@m).
 215  */
 216 gdouble gts_matrix_determinant (GtsMatrix * m)
 217 {
 218   gdouble ans4;
 219   gdouble a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;
 220
 221   g_return_val_if_fail (m != NULL, 0.0);
 222
 223   a1 = m[0][0]; b1 = m[0][1];
 224   c1 = m[0][2]; d1 = m[0][3];
 225
 226   a2 = m[1][0]; b2 = m[1][1];
 227   c2 = m[1][2]; d2 = m[1][3];
 228
 229   a3 = m[2][0]; b3 = m[2][1];
 230   c3 = m[2][2]; d3 = m[2][3];
 231
 232   a4 = m[3][0]; b4 = m[3][1];
 233   c4 = m[3][2]; d4 = m[3][3];
 234
 235   ans4 = a1 * det3x3 (b2, b3, b4, c2, c3, c4, d2, d3, d4)
 236     - b1 * det3x3 (a2, a3, a4, c2, c3, c4, d2, d3, d4)
 237     + c1 * det3x3 (a2, a3, a4, b2, b3, b4, d2, d3, d4)
 238     - d1 * det3x3 (a2, a3, a4, b2, b3, b4, c2, c3, c4);
 239
 240   return ans4;
 241 }
 242
 243 /*
 244  *   adjoint( original_matrix, inverse_matrix )
 245  *
 246  *     calculate the adjoint of a 4x4 matrix
 247  *
 248  *      Let  a   denote the minor determinant of matrix A obtained by
 249  *           ij
 250  *
 251  *      deleting the ith row and jth column from A.
 252  *
 253  *                    i+j
 254  *     Let  b   = (-1)    a
 255  *          ij            ji
 256  *
 257  *    The matrix B = (b  ) is the adjoint of A
 258  *                     ij
 259  */
 260 static GtsMatrix * adjoint (GtsMatrix * m)
 261 {
 262   gdouble a1, a2, a3, a4, b1, b2, b3, b4;
 263   gdouble c1, c2, c3, c4, d1, d2, d3, d4;
 264   GtsMatrix * ma;
 265
 266   a1 = m[0][0]; b1 = m[0][1];
 267   c1 = m[0][2]; d1 = m[0][3];
 268
 269   a2 = m[1][0]; b2 = m[1][1];
 270   c2 = m[1][2]; d2 = m[1][3];
 271
 272   a3 = m[2][0]; b3 = m[2][1];
 273   c3 = m[2][2]; d3 = m[2][3];
 274
 275   a4 = m[3][0]; b4 = m[3][1];
 276   c4 = m[3][2]; d4 = m[3][3];
 277
 278   ma = g_malloc (4*sizeof (GtsVector4));
 279
 280   /* row column labeling reversed since we transpose rows & columns */
 281
 282   ma[0][0]  =   det3x3 (b2, b3, b4, c2, c3, c4, d2, d3, d4);
 283   ma[1][0]  = - det3x3 (a2, a3, a4, c2, c3, c4, d2, d3, d4);
 284   ma[2][0]  =   det3x3 (a2, a3, a4, b2, b3, b4, d2, d3, d4);
 285   ma[3][0]  = - det3x3 (a2, a3, a4, b2, b3, b4, c2, c3, c4);
 286
 287   ma[0][1]  = - det3x3 (b1, b3, b4, c1, c3, c4, d1, d3, d4);
 288   ma[1][1]  =   det3x3 (a1, a3, a4, c1, c3, c4, d1, d3, d4);
 289   ma[2][1]  = - det3x3 (a1, a3, a4, b1, b3, b4, d1, d3, d4);
 290   ma[3][1]  =   det3x3 (a1, a3, a4, b1, b3, b4, c1, c3, c4);
 291
 292   ma[0][2]  =   det3x3 (b1, b2, b4, c1, c2, c4, d1, d2, d4);
 293   ma[1][2]  = - det3x3 (a1, a2, a4, c1, c2, c4, d1, d2, d4);
 294   ma[2][2]  =   det3x3 (a1, a2, a4, b1, b2, b4, d1, d2, d4);
 295   ma[3][2]  = - det3x3 (a1, a2, a4, b1, b2, b4, c1, c2, c4);
 296
 297   ma[0][3]  = - det3x3 (b1, b2, b3, c1, c2, c3, d1, d2, d3);
 298   ma[1][3]  =   det3x3 (a1, a2, a3, c1, c2, c3, d1, d2, d3);
 299   ma[2][3]  = - det3x3 (a1, a2, a3, b1, b2, b3, d1, d2, d3);
 300   ma[3][3]  =   det3x3 (a1, a2, a3, b1, b2, b3, c1, c2, c3);
 301
 302   return ma;
 303 }
 304
 305
 306 /**
 307  * gts_matrix_inverse:
 308  * @m: a #GtsMatrix.
 309  *
 310  * Returns: a pointer to a newly created #GtsMatrix inverse of @m or %NULL
 311  * if @m is not invertible.
 312  */
 313 GtsMatrix * gts_matrix_inverse (GtsMatrix * m)
 314 {
 315   GtsMatrix * madj;
 316   gdouble det;
 317   gint i, j;
 318
 319   g_return_val_if_fail (m != NULL, NULL);
 320
 321   det = gts_matrix_determinant (m);
 322   if (det == 0.)
 323     return NULL;
 324
 325   madj = adjoint (m);
 326   for (i = 0; i < 4; i++)
 327     for(j = 0; j < 4; j++)
 328       madj[i][j] /= det;
 329
 330   return madj;
 331 }
 332
 333 /**
 334  * gts_matrix3_inverse:
 335  * @m: a 3x3 #GtsMatrix.
 336  *
 337  * Returns: a pointer to a newly created 3x3 #GtsMatrix inverse of @m or %NULL
 338  * if @m is not invertible.
 339  */
 340 GtsMatrix * gts_matrix3_inverse (GtsMatrix * m)
 341 {
 342   GtsMatrix * mi;
 343   gdouble det;
 344
 345   g_return_val_if_fail (m != NULL, NULL);
 346
 347   det = (m[0][0]*(m[1][1]*m[2][2] - m[2][1]*m[1][2]) -
 348          m[0][1]*(m[1][0]*m[2][2] - m[2][0]*m[1][2]) +
 349          m[0][2]*(m[1][0]*m[2][1] - m[2][0]*m[1][1]));
 350   if (det == 0.0)
 351     return NULL;
 352
 353   mi = g_malloc0 (4*sizeof (GtsVector));
 354
 355   mi[0][0] = (m[1][1]*m[2][2] - m[1][2]*m[2][1])/det;
 356   mi[0][1] = (m[2][1]*m[0][2] - m[0][1]*m[2][2])/det;
 357   mi[0][2] = (m[0][1]*m[1][2] - m[1][1]*m[0][2])/det;
 358   mi[1][0] = (m[1][2]*m[2][0] - m[1][0]*m[2][2])/det;
 359   mi[1][1] = (m[0][0]*m[2][2] - m[2][0]*m[0][2])/det;
 360   mi[1][2] = (m[1][0]*m[0][2] - m[0][0]*m[1][2])/det;
 361   mi[2][0] = (m[1][0]*m[2][1] - m[2][0]*m[1][1])/det;
 362   mi[2][1] = (m[2][0]*m[0][1] - m[0][0]*m[2][1])/det;
 363   mi[2][2] = (m[0][0]*m[1][1] - m[0][1]*m[1][0])/det;
 364
 365   return mi;
 366 }
 367
 368 /**
 369  * gts_matrix_print:
 370  * @m: a #GtsMatrix.
 371  * @fptr: a file descriptor.
 372  *
 373  * Print @m to file @fptr.
 374  */
 375 void gts_matrix_print (GtsMatrix * m, FILE * fptr)
 376 {
 377   g_return_if_fail (m != NULL);
 378   g_return_if_fail (fptr != NULL);
 379
 380   fprintf (fptr,
 381            "[[%15.7g %15.7g %15.7g %15.7g]\n"
 382            " [%15.7g %15.7g %15.7g %15.7g]\n"
 383            " [%15.7g %15.7g %15.7g %15.7g]\n"
 384            " [%15.7g %15.7g %15.7g %15.7g]]\n",
 385            m[0][0], m[0][1], m[0][2], m[0][3],
 386            m[1][0], m[1][1], m[1][2], m[1][3],
 387            m[2][0], m[2][1], m[2][2], m[2][3],
 388            m[3][0], m[3][1], m[3][2], m[3][3]);
 389 }
 390
 391 /**
 392  * gts_vector_print:
 393  * @v: a #GtsVector.
 394  * @fptr: a file descriptor.
 395  *
 396  * Print @s to file @fptr.
 397  */
 398 void gts_vector_print (GtsVector v, FILE * fptr)
 399 {
 400   g_return_if_fail (fptr != NULL);
 401
 402   fprintf (fptr,
 403            "[%15.7g %15.7g %15.7g ]\n",
 404            v[0], v[1], v[2]);
 405 }
 406
 407 /**
 408  * gts_vector4_print:
 409  * @v: a #GtsVector4.
 410  * @fptr: a file descriptor.
 411  *
 412  * Print @v to file @fptr.
 413  */
 414 void gts_vector4_print (GtsVector4 v, FILE * fptr)
 415 {
 416   g_return_if_fail (fptr != NULL);
 417
 418   fprintf (fptr,
 419            "[%15.7g %15.7g %15.7g %15.7g]\n",
 420            v[0], v[1], v[2], v[3]);
 421 }
 422
 423 /* [cos(alpha)]^2 */
 424 #define COSALPHA2 0.999695413509 /* alpha = 1 degree */
 425 /* [sin(alpha)]^2 */
 426 #define SINALPHA2 3.04586490453e-4 /* alpha = 1 degree */
 427
 428 /**
 429  * gts_matrix_compatible_row:
 430  * @A: a #GtsMatrix.
 431  * @b: a #GtsVector.
 432  * @n: the number of previous constraints of @A.x=@b.
 433  * @A1: a #GtsMatrix.
 434  * @b1: a #GtsVector.
 435  *
 436  * Given a system of @n constraints @A.x=@b adds to it the compatible
 437  * constraints defined by @A1.x=@b1. The compatibility is determined
 438  * by insuring that the resulting system is well-conditioned (see
 439  * Lindstrom and Turk (1998, 1999)).
 440  *
 441  * Returns: the number of constraints of the resulting system.
 442  */
 443 guint gts_matrix_compatible_row (GtsMatrix * A,
 444                                  GtsVector b,
 445                                  guint n,
 446                                  GtsVector A1,
 447                                  gdouble b1)
 448 {
 449   gdouble na1;
 450
 451   g_return_val_if_fail (A != NULL, 0);
 452
 453   na1 = gts_vector_scalar (A1, A1);
 454   if (na1 == 0.0)
 455     return n;
 456
 457   /* normalize row */
 458   na1 = sqrt (na1);
 459   A1[0] /= na1; A1[1] /= na1; A1[2] /= na1; b1 /= na1;
 460
 461   if (n == 1) {
 462     gdouble a0a1 = gts_vector_scalar (A[0], A1);
 463     if (a0a1*a0a1 >= COSALPHA2)
 464       return 1;
 465   }
 466   else if (n == 2) {
 467     GtsVector V;
 468     gdouble s;
 469
 470     gts_vector_cross (V, A[0], A[1]);
 471     s = gts_vector_scalar (V, A1);
 472     if (s*s <= gts_vector_scalar (V, V)*SINALPHA2)
 473       return 2;
 474   }
 475
 476   A[n][0] = A1[0]; A[n][1] = A1[1]; A[n][2] = A1[2]; b[n] = b1;
 477   return n + 1;
 478 }
 479
 480 /**
 481  * gts_matrix_quadratic_optimization:
 482  * @A: a #GtsMatrix.
 483  * @b: a #GtsVector.
 484  * @n: the number of constraints (must be smaller than 3).
 485  * @H: a symmetric positive definite Hessian.
 486  * @c: a #GtsVector.
 487  *
 488  * Solve a quadratic optimization problem: Given a quadratic objective function
 489  * f which can be written as: f(x) = x^t.@H.x + @c^t.x + k, where @H is the
 490  * symmetric positive definite Hessian of f and k is a constant, find the
 491  * minimum of f subject to the set of @n prior linear constraints, defined by
 492  * the first @n rows of @A and @b (@A.x = @b). The new constraints given by
 493  * the minimization are added to @A and @b only if they are linearly
 494  * independent as determined by gts_matrix_compatible_row().
 495  *
 496  * Returns: the new number of constraints defined by @A and @b.
 497  */
 498 guint gts_matrix_quadratic_optimization (GtsMatrix * A,
 499                                          GtsVector b,
 500                                          guint n,
 501                                          GtsMatrix * H,
 502                                          GtsVector c)
 503 {
 504   g_return_val_if_fail (A != NULL, 0);
 505   g_return_val_if_fail (b != NULL, 0);
 506   g_return_val_if_fail (n < 3, 0);
 507   g_return_val_if_fail (H != NULL, 0);
 508
 509   switch (n) {
 510   case 0: {
 511     n = gts_matrix_compatible_row (A, b, n, H[0], - c[0]);
 512     n = gts_matrix_compatible_row (A, b, n, H[1], - c[1]);
 513     n = gts_matrix_compatible_row (A, b, n, H[2], - c[2]);
 514     return n;
 515   }
 516   case 1: {
 517     GtsVector Q0 = {0., 0., 0.};
 518     GtsVector Q1 = {0., 0., 0.};
 519     GtsVector A1;
 520     gdouble max = A[0][0]*A[0][0];
 521     guint d = 0;
 522
 523     /* build a vector orthogonal to the constraint */
 524     if (A[0][1]*A[0][1] > max) { max = A[0][1]*A[0][1]; d = 1; }
 525     if (A[0][2]*A[0][2] > max) { max = A[0][2]*A[0][2]; d = 2; }
 526     switch (d) {
 527     case 0: Q0[0] = - A[0][2]/A[0][0]; Q0[2] = 1.0; break;
 528     case 1: Q0[1] = - A[0][2]/A[0][1]; Q0[2] = 1.0; break;
 529     case 2: Q0[2] = - A[0][0]/A[0][2]; Q0[0] = 1.0; break;
 530     }
 531
 532     /* build a second vector orthogonal to the first and to the constraint */
 533     gts_vector_cross (Q1, A[0], Q0);
 534
 535     A1[0] = gts_vector_scalar (Q0, H[0]);
 536     A1[1] = gts_vector_scalar (Q0, H[1]);
 537     A1[2] = gts_vector_scalar (Q0, H[2]);
 538
 539     n = gts_matrix_compatible_row (A, b, n, A1, - gts_vector_scalar (Q0, c));
 540
 541     A1[0] = gts_vector_scalar (Q1, H[0]);
 542     A1[1] = gts_vector_scalar (Q1, H[1]);
 543     A1[2] = gts_vector_scalar (Q1, H[2]);
 544
 545     n = gts_matrix_compatible_row (A, b, n, A1, - gts_vector_scalar (Q1, c));
 546
 547     return n;
 548   }
 549   case 2: {
 550     /* build a vector orthogonal to the two constraints */
 551     GtsVector A1, Q;
 552
 553     gts_vector_cross (Q, A[0], A[1]);
 554     A1[0] = gts_vector_scalar (Q, H[0]);
 555     A1[1] = gts_vector_scalar (Q, H[1]);
 556     A1[2] = gts_vector_scalar (Q, H[2]);
 557
 558     n = gts_matrix_compatible_row (A, b, n, A1, - gts_vector_scalar (Q, c));
 559
 560     return n;
 561   }
 562   default:
 563     g_assert_not_reached ();
 564   }
 565   return 0;
 566 }
 567
 568 /**
 569  * gts_matrix_destroy:
 570  * @m: a #GtsMatrix.
 571  *
 572  * Free all the memory allocated for @m.
 573  */
 574 void gts_matrix_destroy (GtsMatrix * m)
 575 {
 576   g_free (m);
 577 }
 578
 579 /**
 580  * gts_matrix_product:
 581  * @m1: a #GtsMatrix.
 582  * @m2: another #GtsMatrix.
 583  *
 584  * Returns: a new #GtsMatrix, product of @m1 and @m2.
 585  */
 586 GtsMatrix * gts_matrix_product (GtsMatrix * m1, GtsMatrix * m2)
 587 {
 588   guint i, j;
 589   GtsMatrix * m;
 590
 591   g_return_val_if_fail (m1 != NULL, NULL);
 592   g_return_val_if_fail (m2 != NULL, NULL);
 593   g_return_val_if_fail (m1 != m2, NULL);
 594
 595   m = g_malloc (4*sizeof (GtsVector4));
 596
 597   for (i = 0; i < 4; i++)
 598     for (j = 0; j < 4; j++)
 599       m[i][j] = m1[i][0]*m2[0][j] + m1[i][1]*m2[1][j] +
 600         m1[i][2]*m2[2][j] + m1[i][3]*m2[3][j];
 601   return m;
 602 }
 603
 604 /**
 605  * gts_matrix_zero:
 606  * @m: a #GtsMatrix or $NULL.
 607  *
 608  * Initializes @m to zeros. Allocates a matrix if @m is %NULL.
 609  *
 610  * Returns: the zero'ed matrix.
 611  */
 612 GtsMatrix * gts_matrix_zero (GtsMatrix * m)
 613 {
 614   if (m == NULL)
 615     m = g_malloc0 (4*sizeof (GtsVector4));
 616   else {
 617     m[0][0] = m[1][0] = m[2][0] = m[3][0] = 0.;
 618     m[0][1] = m[1][1] = m[2][1] = m[3][1] = 0.;
 619     m[0][2] = m[1][2] = m[2][2] = m[3][2] = 0.;
 620     m[0][3] = m[1][3] = m[2][3] = m[3][3] = 0.;
 621   }
 622   return m;
 623 }
 624
 625 /**
 626  * gts_matrix_identity:
 627  * @m: a #GtsMatrix or %NULL.
 628  *
 629  * Initializes @m to an identity matrix. Allocates a matrix if @m is %NULL.
 630  *
 631  * Returns: the identity matrix.
 632  */
 633 GtsMatrix * gts_matrix_identity (GtsMatrix * m)
 634 {
 635   m = gts_matrix_zero (m);
 636   m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.;
 637   return m;
 638 }
 639
 640 /**
 641  * gts_matrix_scale:
 642  * @m: a #GtsMatrix or %NULL.
 643  * @s: the scaling vector.
 644  *
 645  * Initializes @m to a scaling matrix for @s. Allocates a matrix if @m
 646  * is %NULL.
 647  *
 648  * Returns: the scaling matrix.
 649  */
 650 GtsMatrix * gts_matrix_scale (GtsMatrix * m, GtsVector s)
 651 {
 652   m = gts_matrix_zero (m);
 653   m[0][0] = s[0];
 654   m[1][1] = s[1];
 655   m[2][2] = s[2];
 656   m[3][3] = 1.;
 657   return m;
 658 }
 659
 660 /**
 661  * gts_matrix_translate:
 662  * @m: a #GtsMatrix or %NULL.
 663  * @t: the translation vector.
 664  *
 665  * Initializes @m to a translation matrix for @t.  Allocates a new
 666  * matrix if @m is %NULL.
 667  *
 668  * Returns: the translation matix.
 669  */
 670 GtsMatrix * gts_matrix_translate (GtsMatrix * m, GtsVector t)
 671 {
 672   m = gts_matrix_zero (m);
 673   m[0][3] = t[0];
 674   m[1][3] = t[1];
 675   m[2][3] = t[2];
 676   m[3][3] = 1.;
 677   m[0][0] = m[1][1] = m[2][2] = 1.;
 678   return m;
 679 }
 680
 681 /**
 682  * gts_matrix_rotate:
 683  * @m: a #GtsMatrix or %NULL.
 684  * @r: the rotation axis.
 685  * @angle: the angle (in radians) to rotate by.
 686  *
 687  * Initializes @m to a rotation matrix around @r by @angle.
 688  * Allocates a new matrix if @m is %NULL.
 689  *
 690  * Returns: the rotation matrix.
 691  */
 692 GtsMatrix * gts_matrix_rotate (GtsMatrix * m,
 693                                GtsVector r,
 694                                gdouble angle)
 695 {
 696   gdouble c, c1, s;
 697
 698   gts_vector_normalize (r);
 699
 700   c = cos (angle);
 701   c1 = 1. - c;
 702   s = sin (angle);
 703
 704   if (m == NULL)
 705     m = g_malloc (4*sizeof (GtsVector4));
 706
 707   m[0][0] = r[0]*r[0]*c1 + c;
 708   m[0][1] = r[0]*r[1]*c1 - r[2]*s;
 709   m[0][2] = r[0]*r[2]*c1 + r[1]*s;
 710   m[0][3] = 0.;
 711
 712   m[1][0] = r[1]*r[0]*c1 + r[2]*s;
 713   m[1][1] = r[1]*r[1]*c1 + c;
 714   m[1][2] = r[1]*r[2]*c1 - r[0]*s;
 715   m[1][3] = 0.;
 716
 717   m[2][0] = r[2]*r[0]*c1 - r[1]*s;
 718   m[2][1] = r[2]*r[1]*c1 + r[0]*s;
 719   m[2][2] = r[2]*r[2]*c1 + c;
 720   m[2][3] = 0.;
 721
 722   m[3][0] = 0.;
 723   m[3][1] = 0.;
 724   m[3][2] = 0.;
 725   m[3][3] = 1.;
 726
 727   return m;
 728 }