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 */
gts_matrix_new(gdouble a00,gdouble a01,gdouble a02,gdouble a03,gdouble a10,gdouble a11,gdouble a12,gdouble a13,gdouble a20,gdouble a21,gdouble a22,gdouble a23,gdouble a30,gdouble a31,gdouble a32,gdouble a33)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 */
gts_matrix_assign(GtsMatrix * m,gdouble a00,gdouble a01,gdouble a02,gdouble a03,gdouble a10,gdouble a11,gdouble a12,gdouble a13,gdouble a20,gdouble a21,gdouble a22,gdouble a23,gdouble a30,gdouble a31,gdouble a32,gdouble a33)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 */
gts_matrix_projection(GtsTriangle * t)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 */
gts_matrix_transpose(GtsMatrix * m)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 */
det2x2(gdouble a,gdouble b,gdouble c,gdouble d)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 */
det3x3(gdouble a1,gdouble a2,gdouble a3,gdouble b1,gdouble b2,gdouble b3,gdouble c1,gdouble c2,gdouble c3)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 */
gts_matrix_determinant(GtsMatrix * m)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 */
adjoint(GtsMatrix * m)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 */
gts_matrix_inverse(GtsMatrix * m)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 */
gts_matrix3_inverse(GtsMatrix * m)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 */
gts_matrix_print(GtsMatrix * m,FILE * fptr)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 */
gts_vector_print(GtsVector v,FILE * fptr)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 */
gts_vector4_print(GtsVector4 v,FILE * fptr)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 */
gts_matrix_compatible_row(GtsMatrix * A,GtsVector b,guint n,GtsVector A1,gdouble b1)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 */
gts_matrix_quadratic_optimization(GtsMatrix * A,GtsVector b,guint n,GtsMatrix * H,GtsVector c)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 */
gts_matrix_destroy(GtsMatrix * m)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 */
gts_matrix_product(GtsMatrix * m1,GtsMatrix * m2)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 */
gts_matrix_zero(GtsMatrix * m)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 */
gts_matrix_identity(GtsMatrix * m)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 */
gts_matrix_scale(GtsMatrix * m,GtsVector s)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 */
gts_matrix_translate(GtsMatrix * m,GtsVector t)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 */
gts_matrix_rotate(GtsMatrix * m,GtsVector r,gdouble angle)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 }
726