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   g_assert ((l = sqrt (x1*x1 + y1*y1 + z1*z1)) > 0.0);
 130   m[0][0] = x1/l; m[1][0] = y1/l; m[2][0] = z1/l; m[3][0] = 0.;
 131   g_assert ((l = sqrt (x2*x2 + y2*y2 + z2*z2)) > 0.0);
 132   m[0][1] = x2/l; m[1][1] = y2/l; m[2][1] = z2/l; m[3][1] = 0.;
 133   g_assert ((l = sqrt (x3*x3 + y3*y3 + z3*z3)) > 0.0);
 134   m[0][2] = x3/l; m[1][2] = y3/l; m[2][2] = z3/l; m[3][2] = 0.;
 135   m[0][3] = 0; m[1][3] = 0.; m[2][3] = 0.; m[3][3] = 1.;
 136
 137   return m;
 138 }
 139
 140 /**
 141  * gts_matrix_transpose:
 142  * @m: a #GtsMatrix.
 143  *
 144  * Returns: a pointer to a newly created #GtsMatrix transposed of @m.
 145  */
 146 GtsMatrix * gts_matrix_transpose (GtsMatrix * m)
 147 {
 148   GtsMatrix * mi;
 149
 150   g_return_val_if_fail (m != NULL, NULL);
 151
 152   mi = g_malloc (4*sizeof (GtsVector4));
 153
 154   mi[0][0] = m[0][0]; mi[1][0] = m[0][1];
 155   mi[2][0] = m[0][2]; mi[3][0] = m[0][3];
 156   mi[0][1] = m[1][0]; mi[1][1] = m[1][1];
 157   mi[2][1] = m[1][2]; mi[3][1] = m[1][3];
 158   mi[0][2] = m[2][0]; mi[1][2] = m[2][1];
 159   mi[2][2] = m[2][2]; mi[3][2] = m[2][3];
 160   mi[0][3] = m[3][0]; mi[1][3] = m[3][1];
 161   mi[2][3] = m[3][2]; mi[3][3] = m[3][3];
 162
 163   return mi;
 164 }
 165
 166 /*
 167  * calculate the determinant of a 2x2 matrix.
 168  *
 169  * Adapted from:
 170  * Matrix Inversion
 171  * by Richard Carling
 172  * from "Graphics Gems", Academic Press, 1990
 173  */
 174 static gdouble det2x2 (gdouble a, gdouble b, gdouble c, gdouble d)
 175 {
 176   gdouble ans2;
 177
 178   ans2 = a*d - b*c;
 179   return ans2;
 180 }
 181
 182 /*
 183  * calculate the determinant of a 3x3 matrix
 184  * in the form
 185  *
 186  *     | a1,  b1,  c1 |
 187  *     | a2,  b2,  c2 |
 188  *     | a3,  b3,  c3 |
 189  *
 190  * Adapted from:
 191  * Matrix Inversion
 192  * by Richard Carling
 193  * from "Graphics Gems", Academic Press, 1990
 194  */
 195 static gdouble det3x3 (gdouble a1, gdouble a2, gdouble a3,
 196                        gdouble b1, gdouble b2, gdouble b3,
 197                        gdouble c1, gdouble c2, gdouble c3)
 198 {
 199   gdouble ans3;
 200
 201   ans3 = a1 * det2x2( b2, b3, c2, c3 )
 202     - b1 * det2x2( a2, a3, c2, c3 )
 203     + c1 * det2x2( a2, a3, b2, b3 );
 204   return ans3;
 205 }
 206
 207 /**
 208  * gts_matrix_determinant:
 209  * @m: a #GtsMatrix.
 210  *
 211  * Returns: the value of det(@m).
 212  */
 213 gdouble gts_matrix_determinant (GtsMatrix * m)
 214 {
 215   gdouble ans4;
 216   gdouble a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;
 217
 218   g_return_val_if_fail (m != NULL, 0.0);
 219
 220   a1 = m[0][0]; b1 = m[0][1];
 221   c1 = m[0][2]; d1 = m[0][3];
 222
 223   a2 = m[1][0]; b2 = m[1][1];
 224   c2 = m[1][2]; d2 = m[1][3];
 225
 226   a3 = m[2][0]; b3 = m[2][1];
 227   c3 = m[2][2]; d3 = m[2][3];
 228
 229   a4 = m[3][0]; b4 = m[3][1];
 230   c4 = m[3][2]; d4 = m[3][3];
 231
 232   ans4 = a1 * det3x3 (b2, b3, b4, c2, c3, c4, d2, d3, d4)
 233     - b1 * det3x3 (a2, a3, a4, c2, c3, c4, d2, d3, d4)
 234     + c1 * det3x3 (a2, a3, a4, b2, b3, b4, d2, d3, d4)
 235     - d1 * det3x3 (a2, a3, a4, b2, b3, b4, c2, c3, c4);
 236
 237   return ans4;
 238 }
 239
 240 /*
 241  *   adjoint( original_matrix, inverse_matrix )
 242  *
 243  *     calculate the adjoint of a 4x4 matrix
 244  *
 245  *      Let  a   denote the minor determinant of matrix A obtained by
 246  *           ij
 247  *
 248  *      deleting the ith row and jth column from A.
 249  *
 250  *                    i+j
 251  *     Let  b   = (-1)    a
 252  *          ij            ji
 253  *
 254  *    The matrix B = (b  ) is the adjoint of A
 255  *                     ij
 256  */
 257 static GtsMatrix * adjoint (GtsMatrix * m)
 258 {
 259   gdouble a1, a2, a3, a4, b1, b2, b3, b4;
 260   gdouble c1, c2, c3, c4, d1, d2, d3, d4;
 261   GtsMatrix * ma;
 262
 263   a1 = m[0][0]; b1 = m[0][1];
 264   c1 = m[0][2]; d1 = m[0][3];
 265
 266   a2 = m[1][0]; b2 = m[1][1];
 267   c2 = m[1][2]; d2 = m[1][3];
 268
 269   a3 = m[2][0]; b3 = m[2][1];
 270   c3 = m[2][2]; d3 = m[2][3];
 271
 272   a4 = m[3][0]; b4 = m[3][1];
 273   c4 = m[3][2]; d4 = m[3][3];
 274
 275   ma = g_malloc (4*sizeof (GtsVector4));
 276
 277   /* row column labeling reversed since we transpose rows & columns */
 278
 279   ma[0][0]  =   det3x3 (b2, b3, b4, c2, c3, c4, d2, d3, d4);
 280   ma[1][0]  = - det3x3 (a2, a3, a4, c2, c3, c4, d2, d3, d4);
 281   ma[2][0]  =   det3x3 (a2, a3, a4, b2, b3, b4, d2, d3, d4);
 282   ma[3][0]  = - det3x3 (a2, a3, a4, b2, b3, b4, c2, c3, c4);
 283
 284   ma[0][1]  = - det3x3 (b1, b3, b4, c1, c3, c4, d1, d3, d4);
 285   ma[1][1]  =   det3x3 (a1, a3, a4, c1, c3, c4, d1, d3, d4);
 286   ma[2][1]  = - det3x3 (a1, a3, a4, b1, b3, b4, d1, d3, d4);
 287   ma[3][1]  =   det3x3 (a1, a3, a4, b1, b3, b4, c1, c3, c4);
 288
 289   ma[0][2]  =   det3x3 (b1, b2, b4, c1, c2, c4, d1, d2, d4);
 290   ma[1][2]  = - det3x3 (a1, a2, a4, c1, c2, c4, d1, d2, d4);
 291   ma[2][2]  =   det3x3 (a1, a2, a4, b1, b2, b4, d1, d2, d4);
 292   ma[3][2]  = - det3x3 (a1, a2, a4, b1, b2, b4, c1, c2, c4);
 293
 294   ma[0][3]  = - det3x3 (b1, b2, b3, c1, c2, c3, d1, d2, d3);
 295   ma[1][3]  =   det3x3 (a1, a2, a3, c1, c2, c3, d1, d2, d3);
 296   ma[2][3]  = - det3x3 (a1, a2, a3, b1, b2, b3, d1, d2, d3);
 297   ma[3][3]  =   det3x3 (a1, a2, a3, b1, b2, b3, c1, c2, c3);
 298
 299   return ma;
 300 }
 301
 302
 303 /**
 304  * gts_matrix_inverse:
 305  * @m: a #GtsMatrix.
 306  *
 307  * Returns: a pointer to a newly created #GtsMatrix inverse of @m or %NULL
 308  * if @m is not invertible.
 309  */
 310 GtsMatrix * gts_matrix_inverse (GtsMatrix * m)
 311 {
 312   GtsMatrix * madj;
 313   gdouble det;
 314   gint i, j;
 315
 316   g_return_val_if_fail (m != NULL, NULL);
 317
 318   det = gts_matrix_determinant (m);
 319   if (det == 0.)
 320     return NULL;
 321
 322   madj = adjoint (m);
 323   for (i = 0; i < 4; i++)
 324     for(j = 0; j < 4; j++)
 325       madj[i][j] /= det;
 326
 327   return madj;
 328 }
 329
 330 /**
 331  * gts_matrix3_inverse:
 332  * @m: a 3x3 #GtsMatrix.
 333  *
 334  * Returns: a pointer to a newly created 3x3 #GtsMatrix inverse of @m or %NULL
 335  * if @m is not invertible.
 336  */
 337 GtsMatrix * gts_matrix3_inverse (GtsMatrix * m)
 338 {
 339   GtsMatrix * mi;
 340   gdouble det;
 341
 342   g_return_val_if_fail (m != NULL, NULL);
 343
 344   det = (m[0][0]*(m[1][1]*m[2][2] - m[2][1]*m[1][2]) -
 345          m[0][1]*(m[1][0]*m[2][2] - m[2][0]*m[1][2]) +
 346          m[0][2]*(m[1][0]*m[2][1] - m[2][0]*m[1][1]));
 347   if (det == 0.0)
 348     return NULL;
 349
 350   mi = g_malloc0 (4*sizeof (GtsVector));
 351
 352   mi[0][0] = (m[1][1]*m[2][2] - m[1][2]*m[2][1])/det;
 353   mi[0][1] = (m[2][1]*m[0][2] - m[0][1]*m[2][2])/det;
 354   mi[0][2] = (m[0][1]*m[1][2] - m[1][1]*m[0][2])/det;
 355   mi[1][0] = (m[1][2]*m[2][0] - m[1][0]*m[2][2])/det;
 356   mi[1][1] = (m[0][0]*m[2][2] - m[2][0]*m[0][2])/det;
 357   mi[1][2] = (m[1][0]*m[0][2] - m[0][0]*m[1][2])/det;
 358   mi[2][0] = (m[1][0]*m[2][1] - m[2][0]*m[1][1])/det;
 359   mi[2][1] = (m[2][0]*m[0][1] - m[0][0]*m[2][1])/det;
 360   mi[2][2] = (m[0][0]*m[1][1] - m[0][1]*m[1][0])/det;
 361
 362   return mi;
 363 }
 364
 365 /**
 366  * gts_matrix_print:
 367  * @m: a #GtsMatrix.
 368  * @fptr: a file descriptor.
 369  *
 370  * Print @m to file @fptr.
 371  */
 372 void gts_matrix_print (GtsMatrix * m, FILE * fptr)
 373 {
 374   g_return_if_fail (m != NULL);
 375   g_return_if_fail (fptr != NULL);
 376
 377   fprintf (fptr,
 378            "[[%15.7g %15.7g %15.7g %15.7g]\n"
 379            " [%15.7g %15.7g %15.7g %15.7g]\n"
 380            " [%15.7g %15.7g %15.7g %15.7g]\n"
 381            " [%15.7g %15.7g %15.7g %15.7g]]\n",
 382            m[0][0], m[0][1], m[0][2], m[0][3],
 383            m[1][0], m[1][1], m[1][2], m[1][3],
 384            m[2][0], m[2][1], m[2][2], m[2][3],
 385            m[3][0], m[3][1], m[3][2], m[3][3]);
 386 }
 387
 388 /**
 389  * gts_vector_print:
 390  * @v: a #GtsVector.
 391  * @fptr: a file descriptor.
 392  *
 393  * Print @s to file @fptr.
 394  */
 395 void gts_vector_print (GtsVector v, FILE * fptr)
 396 {
 397   g_return_if_fail (fptr != NULL);
 398
 399   fprintf (fptr,
 400            "[%15.7g %15.7g %15.7g ]\n",
 401            v[0], v[1], v[2]);
 402 }
 403
 404 /**
 405  * gts_vector4_print:
 406  * @v: a #GtsVector4.
 407  * @fptr: a file descriptor.
 408  *
 409  * Print @v to file @fptr.
 410  */
 411 void gts_vector4_print (GtsVector4 v, FILE * fptr)
 412 {
 413   g_return_if_fail (fptr != NULL);
 414
 415   fprintf (fptr,
 416            "[%15.7g %15.7g %15.7g %15.7g]\n",
 417            v[0], v[1], v[2], v[3]);
 418 }
 419
 420 /* [cos(alpha)]^2 */
 421 #define COSALPHA2 0.999695413509 /* alpha = 1 degree */
 422 /* [sin(alpha)]^2 */
 423 #define SINALPHA2 3.04586490453e-4 /* alpha = 1 degree */
 424
 425 /**
 426  * gts_matrix_compatible_row:
 427  * @A: a #GtsMatrix.
 428  * @b: a #GtsVector.
 429  * @n: the number of previous constraints of @A.x=@b.
 430  * @A1: a #GtsMatrix.
 431  * @b1: a #GtsVector.
 432  *
 433  * Given a system of @n constraints @A.x=@b adds to it the compatible
 434  * constraints defined by @A1.x=@b1. The compatibility is determined
 435  * by insuring that the resulting system is well-conditioned (see
 436  * Lindstrom and Turk (1998, 1999)).
 437  *
 438  * Returns: the number of constraints of the resulting system.
 439  */
 440 guint gts_matrix_compatible_row (GtsMatrix * A,
 441                                  GtsVector b,
 442                                  guint n,
 443                                  GtsVector A1,
 444                                  gdouble b1)
 445 {
 446   gdouble na1;
 447
 448   g_return_val_if_fail (A != NULL, 0);
 449
 450   na1 = gts_vector_scalar (A1, A1);
 451   if (na1 == 0.0)
 452     return n;
 453
 454   /* normalize row */
 455   na1 = sqrt (na1);
 456   A1[0] /= na1; A1[1] /= na1; A1[2] /= na1; b1 /= na1;
 457
 458   if (n == 1) {
 459     gdouble a0a1 = gts_vector_scalar (A[0], A1);
 460     if (a0a1*a0a1 >= COSALPHA2)
 461       return 1;
 462   }
 463   else if (n == 2) {
 464     GtsVector V;
 465     gdouble s;
 466
 467     gts_vector_cross (V, A[0], A[1]);
 468     s = gts_vector_scalar (V, A1);
 469     if (s*s <= gts_vector_scalar (V, V)*SINALPHA2)
 470       return 2;
 471   }
 472
 473   A[n][0] = A1[0]; A[n][1] = A1[1]; A[n][2] = A1[2]; b[n] = b1;
 474   return n + 1;
 475 }
 476
 477 /**
 478  * gts_matrix_quadratic_optimization:
 479  * @A: a #GtsMatrix.
 480  * @b: a #GtsVector.
 481  * @n: the number of constraints (must be smaller than 3).
 482  * @H: a symmetric positive definite Hessian.
 483  * @c: a #GtsVector.
 484  *
 485  * Solve a quadratic optimization problem: Given a quadratic objective function
 486  * f which can be written as: f(x) = x^t.@H.x + @c^t.x + k, where @H is the
 487  * symmetric positive definite Hessian of f and k is a constant, find the
 488  * minimum of f subject to the set of @n prior linear constraints, defined by
 489  * the first @n rows of @A and @b (@A.x = @b). The new constraints given by
 490  * the minimization are added to @A and @b only if they are linearly
 491  * independent as determined by gts_matrix_compatible_row().
 492  *
 493  * Returns: the new number of constraints defined by @A and @b.
 494  */
 495 guint gts_matrix_quadratic_optimization (GtsMatrix * A,
 496                                          GtsVector b,
 497                                          guint n,
 498                                          GtsMatrix * H,
 499                                          GtsVector c)
 500 {
 501   g_return_val_if_fail (A != NULL, 0);
 502   g_return_val_if_fail (b != NULL, 0);
 503   g_return_val_if_fail (n < 3, 0);
 504   g_return_val_if_fail (H != NULL, 0);
 505
 506   switch (n) {
 507   case 0: {
 508     n = gts_matrix_compatible_row (A, b, n, H[0], - c[0]);
 509     n = gts_matrix_compatible_row (A, b, n, H[1], - c[1]);
 510     n = gts_matrix_compatible_row (A, b, n, H[2], - c[2]);
 511     return n;
 512   }
 513   case 1: {
 514     GtsVector Q0 = {0., 0., 0.};
 515     GtsVector Q1 = {0., 0., 0.};
 516     GtsVector A1;
 517     gdouble max = A[0][0]*A[0][0];
 518     guint d = 0;
 519
 520     /* build a vector orthogonal to the constraint */
 521     if (A[0][1]*A[0][1] > max) { max = A[0][1]*A[0][1]; d = 1; }
 522     if (A[0][2]*A[0][2] > max) { max = A[0][2]*A[0][2]; d = 2; }
 523     switch (d) {
 524     case 0: Q0[0] = - A[0][2]/A[0][0]; Q0[2] = 1.0; break;
 525     case 1: Q0[1] = - A[0][2]/A[0][1]; Q0[2] = 1.0; break;
 526     case 2: Q0[2] = - A[0][0]/A[0][2]; Q0[0] = 1.0; break;
 527     }
 528
 529     /* build a second vector orthogonal to the first and to the constraint */
 530     gts_vector_cross (Q1, A[0], Q0);
 531
 532     A1[0] = gts_vector_scalar (Q0, H[0]);
 533     A1[1] = gts_vector_scalar (Q0, H[1]);
 534     A1[2] = gts_vector_scalar (Q0, H[2]);
 535
 536     n = gts_matrix_compatible_row (A, b, n, A1, - gts_vector_scalar (Q0, c));
 537
 538     A1[0] = gts_vector_scalar (Q1, H[0]);
 539     A1[1] = gts_vector_scalar (Q1, H[1]);
 540     A1[2] = gts_vector_scalar (Q1, H[2]);
 541
 542     n = gts_matrix_compatible_row (A, b, n, A1, - gts_vector_scalar (Q1, c));
 543
 544     return n;
 545   }
 546   case 2: {
 547     /* build a vector orthogonal to the two constraints */
 548     GtsVector A1, Q;
 549
 550     gts_vector_cross (Q, A[0], A[1]);
 551     A1[0] = gts_vector_scalar (Q, H[0]);
 552     A1[1] = gts_vector_scalar (Q, H[1]);
 553     A1[2] = gts_vector_scalar (Q, H[2]);
 554
 555     n = gts_matrix_compatible_row (A, b, n, A1, - gts_vector_scalar (Q, c));
 556
 557     return n;
 558   }
 559   default:
 560     g_assert_not_reached ();
 561   }
 562   return 0;
 563 }
 564
 565 /**
 566  * gts_matrix_destroy:
 567  * @m: a #GtsMatrix.
 568  *
 569  * Free all the memory allocated for @m.
 570  */
 571 void gts_matrix_destroy (GtsMatrix * m)
 572 {
 573   g_free (m);
 574 }
 575
 576 /**
 577  * gts_matrix_product:
 578  * @m1: a #GtsMatrix.
 579  * @m2: another #GtsMatrix.
 580  *
 581  * Returns: a new #GtsMatrix, product of @m1 and @m2.
 582  */
 583 GtsMatrix * gts_matrix_product (GtsMatrix * m1, GtsMatrix * m2)
 584 {
 585   guint i, j;
 586   GtsMatrix * m;
 587
 588   g_return_val_if_fail (m1 != NULL, NULL);
 589   g_return_val_if_fail (m2 != NULL, NULL);
 590   g_return_val_if_fail (m1 != m2, NULL);
 591
 592   m = g_malloc (4*sizeof (GtsVector4));
 593
 594   for (i = 0; i < 4; i++)
 595     for (j = 0; j < 4; j++)
 596       m[i][j] = m1[i][0]*m2[0][j] + m1[i][1]*m2[1][j] +
 597         m1[i][2]*m2[2][j] + m1[i][3]*m2[3][j];
 598   return m;
 599 }
 600
 601 /**
 602  * gts_matrix_zero:
 603  * @m: a #GtsMatrix or $NULL.
 604  *
 605  * Initializes @m to zeros. Allocates a matrix if @m is %NULL.
 606  *
 607  * Returns: the zero'ed matrix.
 608  */
 609 GtsMatrix * gts_matrix_zero (GtsMatrix * m)
 610 {
 611   if (m == NULL)
 612     m = g_malloc0 (4*sizeof (GtsVector4));
 613   else {
 614     m[0][0] = m[1][0] = m[2][0] = m[3][0] = 0.;
 615     m[0][1] = m[1][1] = m[2][1] = m[3][1] = 0.;
 616     m[0][2] = m[1][2] = m[2][2] = m[3][2] = 0.;
 617     m[0][3] = m[1][3] = m[2][3] = m[3][3] = 0.;
 618   }
 619   return m;
 620 }
 621
 622 /**
 623  * gts_matrix_identity:
 624  * @m: a #GtsMatrix or %NULL.
 625  *
 626  * Initializes @m to an identity matrix. Allocates a matrix if @m is %NULL.
 627  *
 628  * Returns: the identity matrix.
 629  */
 630 GtsMatrix * gts_matrix_identity (GtsMatrix * m)
 631 {
 632   m = gts_matrix_zero (m);
 633   m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.;
 634   return m;
 635 }
 636
 637 /**
 638  * gts_matrix_scale:
 639  * @m: a #GtsMatrix or %NULL.
 640  * @s: the scaling vector.
 641  *
 642  * Initializes @m to a scaling matrix for @s. Allocates a matrix if @m
 643  * is %NULL.
 644  *
 645  * Returns: the scaling matrix.
 646  */
 647 GtsMatrix * gts_matrix_scale (GtsMatrix * m, GtsVector s)
 648 {
 649   m = gts_matrix_zero (m);
 650   m[0][0] = s[0];
 651   m[1][1] = s[1];
 652   m[2][2] = s[2];
 653   m[3][3] = 1.;
 654   return m;
 655 }
 656
 657 /**
 658  * gts_matrix_translate:
 659  * @m: a #GtsMatrix or %NULL.
 660  * @t: the translation vector.
 661  *
 662  * Initializes @m to a translation matrix for @t.  Allocates a new
 663  * matrix if @m is %NULL.
 664  *
 665  * Returns: the translation matix.
 666  */
 667 GtsMatrix * gts_matrix_translate (GtsMatrix * m, GtsVector t)
 668 {
 669   m = gts_matrix_zero (m);
 670   m[0][3] = t[0];
 671   m[1][3] = t[1];
 672   m[2][3] = t[2];
 673   m[3][3] = 1.;
 674   m[0][0] = m[1][1] = m[2][2] = 1.;
 675   return m;
 676 }
 677
 678 /**
 679  * gts_matrix_rotate:
 680  * @m: a #GtsMatrix or %NULL.
 681  * @r: the rotation axis.
 682  * @angle: the angle (in radians) to rotate by.
 683  *
 684  * Initializes @m to a rotation matrix around @r by @angle.
 685  * Allocates a new matrix if @m is %NULL.
 686  *
 687  * Returns: the rotation matrix.
 688  */
 689 GtsMatrix * gts_matrix_rotate (GtsMatrix * m,
 690                                GtsVector r,
 691                                gdouble angle)
 692 {
 693   gdouble c, c1, s;
 694
 695   gts_vector_normalize (r);
 696
 697   c = cos (angle);
 698   c1 = 1. - c;
 699   s = sin (angle);
 700
 701   if (m == NULL)
 702     m = g_malloc (4*sizeof (GtsVector4));
 703
 704   m[0][0] = r[0]*r[0]*c1 + c;
 705   m[0][1] = r[0]*r[1]*c1 - r[2]*s;
 706   m[0][2] = r[0]*r[2]*c1 + r[1]*s;
 707   m[0][3] = 0.;
 708
 709   m[1][0] = r[1]*r[0]*c1 + r[2]*s;
 710   m[1][1] = r[1]*r[1]*c1 + c;
 711   m[1][2] = r[1]*r[2]*c1 - r[0]*s;
 712   m[1][3] = 0.;
 713
 714   m[2][0] = r[2]*r[0]*c1 - r[1]*s;
 715   m[2][1] = r[2]*r[1]*c1 + r[0]*s;
 716   m[2][2] = r[2]*r[2]*c1 + c;
 717   m[2][3] = 0.;
 718
 719   m[3][0] = 0.;
 720   m[3][1] = 0.;
 721   m[3][2] = 0.;
 722   m[3][3] = 1.;
 723
 724   return m;
 725 }