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 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 */
gts_matrix_transpose(GtsMatrix * m)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 */
det2x2(gdouble a,gdouble b,gdouble c,gdouble d)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 */
det3x3(gdouble a1,gdouble a2,gdouble a3,gdouble b1,gdouble b2,gdouble b3,gdouble c1,gdouble c2,gdouble c3)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 */
gts_matrix_determinant(GtsMatrix * m)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 */
adjoint(GtsMatrix * m)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 */
gts_matrix_inverse(GtsMatrix * m)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 */
gts_matrix3_inverse(GtsMatrix * m)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 */
gts_matrix_print(GtsMatrix * m,FILE * fptr)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 */
gts_vector_print(GtsVector v,FILE * fptr)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 */
gts_vector4_print(GtsVector4 v,FILE * fptr)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 */
gts_matrix_compatible_row(GtsMatrix * A,GtsVector b,guint n,GtsVector A1,gdouble b1)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 */
gts_matrix_quadratic_optimization(GtsMatrix * A,GtsVector b,guint n,GtsMatrix * H,GtsVector c)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 */
gts_matrix_destroy(GtsMatrix * m)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 */
gts_matrix_product(GtsMatrix * m1,GtsMatrix * m2)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 */
gts_matrix_zero(GtsMatrix * m)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 */
gts_matrix_identity(GtsMatrix * m)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 */
gts_matrix_scale(GtsMatrix * m,GtsVector s)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 */
gts_matrix_translate(GtsMatrix * m,GtsVector t)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 */
gts_matrix_rotate(GtsMatrix * m,GtsVector r,gdouble angle)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 }
729