1 /* -*- Mode: C; tab-width: 8; indent-tabs-mode: t; c-basic-offset: 8 -*-
2 *
3 * Copyright (C) 2010-12 Richard Hughes <richard@hughsie.com>
4 *
5 * Licensed under the GNU Lesser General Public License Version 2.1
6 *
7 * This library is free software; you can redistribute it and/or
8 * modify it under the terms of the GNU Lesser General Public
9 * License as published by the Free Software Foundation; either
10 * version 2.1 of the License, or (at your option) any later version.
11 *
12 * This library is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 * Lesser General Public License for more details.
16 *
17 * You should have received a copy of the GNU Lesser General Public
18 * License along with this library; if not, write to the Free Software
19 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
20 */
21
22 /**
23 * SECTION:cd-math
24 * @short_description: Common maths functionality
25 *
26 * A GObject to use for common maths functionality like vectors and matrices.
27 */
28
29 #include "config.h"
30
31 #include <math.h>
32 #include <string.h>
33 #include <glib-object.h>
34
35 #include <cd-math.h>
36
37 /**
38 * cd_vec3_clear:
39 * @src: the source vector
40 *
41 * Clears a vector, setting all it's values to zero.
42 **/
43 void
cd_vec3_clear(CdVec3 * src)44 cd_vec3_clear (CdVec3 *src)
45 {
46 src->v0 = 0.0f;
47 src->v1 = 0.0f;
48 src->v2 = 0.0f;
49 }
50
51 /**
52 * cd_vec3_init:
53 * @dest: the destination vector
54 * @v0: component value
55 * @v1: component value
56 * @v2: component value
57 *
58 * Initialises a vector.
59 **/
60 void
cd_vec3_init(CdVec3 * dest,gdouble v0,gdouble v1,gdouble v2)61 cd_vec3_init (CdVec3 *dest, gdouble v0, gdouble v1, gdouble v2)
62 {
63 g_return_if_fail (dest != NULL);
64
65 dest->v0 = v0;
66 dest->v1 = v1;
67 dest->v2 = v2;
68 }
69
70 /**
71 * cd_vec3_scalar_multiply:
72 * @src: the source
73 * @value: the scalar multiplier
74 * @dest: the destination
75 *
76 * Multiplies a vector with a scalar.
77 * The arguments @src and @dest can be the same value.
78 **/
79 void
cd_vec3_scalar_multiply(const CdVec3 * src,gdouble value,CdVec3 * dest)80 cd_vec3_scalar_multiply (const CdVec3 *src, gdouble value, CdVec3 *dest)
81 {
82 dest->v0 = src->v0 * value;
83 dest->v1 = src->v1 * value;
84 dest->v2 = src->v2 * value;
85 }
86
87 /**
88 * cd_vec3_copy:
89 * @src: the source
90 * @dest: the destination
91 *
92 * Copies the vector into another vector.
93 * The arguments @src and @dest cannot be the same value.
94 **/
95 void
cd_vec3_copy(const CdVec3 * src,CdVec3 * dest)96 cd_vec3_copy (const CdVec3 *src, CdVec3 *dest)
97 {
98 g_return_if_fail (src != dest);
99 memcpy (dest, src, sizeof (CdVec3));
100 }
101
102 /**
103 * cd_vec3_add:
104 * @src1: the source
105 * @src2: the other source
106 * @dest: the destination
107 *
108 * Adds two vector quantaties
109 * The arguments @src and @dest can be the same value.
110 **/
111 void
cd_vec3_add(const CdVec3 * src1,const CdVec3 * src2,CdVec3 * dest)112 cd_vec3_add (const CdVec3 *src1, const CdVec3 *src2, CdVec3 *dest)
113 {
114 dest->v0 = src1->v0 + src2->v0;
115 dest->v1 = src1->v1 + src2->v1;
116 dest->v2 = src1->v2 + src2->v2;
117 }
118
119 /**
120 * cd_vec3_subtract:
121 * @src1: the source
122 * @src2: the other source
123 * @dest: the destination
124 *
125 * Subtracts one vector quantaty from another
126 * The arguments @src and @dest can be the same value.
127 **/
128 void
cd_vec3_subtract(const CdVec3 * src1,const CdVec3 * src2,CdVec3 * dest)129 cd_vec3_subtract (const CdVec3 *src1, const CdVec3 *src2, CdVec3 *dest)
130 {
131 dest->v0 = src1->v0 - src2->v0;
132 dest->v1 = src1->v1 - src2->v1;
133 dest->v2 = src1->v2 - src2->v2;
134 }
135
136 /**
137 * cd_vec3_to_string:
138 * @src: the source
139 *
140 * Obtains a string representaton of a vector.
141 *
142 * Return value: the string. Free with g_free()
143 **/
144 gchar *
cd_vec3_to_string(const CdVec3 * src)145 cd_vec3_to_string (const CdVec3 *src)
146 {
147 return g_strdup_printf ("\n/ %0 .6f \\\n"
148 "| %0 .6f |\n"
149 "\\ %0 .6f /",
150 src->v0, src->v1, src->v2);
151 }
152
153 /**
154 * cd_vec3_get_data:
155 * @src: the vector source
156 *
157 * Gets the raw data for the vector.
158 *
159 * Return value: the pointer to the data segment.
160 **/
161 gdouble *
cd_vec3_get_data(const CdVec3 * src)162 cd_vec3_get_data (const CdVec3 *src)
163 {
164 return (gdouble *) src;
165 }
166
167 /**
168 * cd_vec3_squared_error:
169 * @src1: the vector source
170 * @src2: another vector source
171 *
172 * Gets the mean squared error for a pair of vectors
173 *
174 * Return value: the floating point MSE.
175 **/
176 gdouble
cd_vec3_squared_error(const CdVec3 * src1,const CdVec3 * src2)177 cd_vec3_squared_error (const CdVec3 *src1, const CdVec3 *src2)
178 {
179 CdVec3 tmp;
180 cd_vec3_subtract (src1, src2, &tmp);
181 return (tmp.v0 * tmp.v0) +
182 (tmp.v1 * tmp.v1) +
183 (tmp.v2 * tmp.v2);
184 }
185
186 /**
187 * cd_mat33_init:
188 * @dest: the destination matrix
189 * @m00: component value
190 * @m01: component value
191 * @m02: component value
192 * @m10: component value
193 * @m11: component value
194 * @m12: component value
195 * @m20: component value
196 * @m21: component value
197 * @m22: component value
198 *
199 * Initialises a matrix.
200 **/
201 void
cd_mat33_init(CdMat3x3 * dest,gdouble m00,gdouble m01,gdouble m02,gdouble m10,gdouble m11,gdouble m12,gdouble m20,gdouble m21,gdouble m22)202 cd_mat33_init (CdMat3x3 *dest,
203 gdouble m00, gdouble m01, gdouble m02,
204 gdouble m10, gdouble m11, gdouble m12,
205 gdouble m20, gdouble m21, gdouble m22)
206 {
207 g_return_if_fail (dest != NULL);
208
209 dest->m00 = m00;
210 dest->m01 = m01;
211 dest->m02 = m02;
212 dest->m10 = m10;
213 dest->m11 = m11;
214 dest->m12 = m12;
215 dest->m20 = m20;
216 dest->m21 = m21;
217 dest->m22 = m22;
218 }
219
220 /**
221 * cd_mat33_clear:
222 * @src: the source
223 *
224 * Clears a matrix value, setting all it's values to zero.
225 **/
226 void
cd_mat33_clear(const CdMat3x3 * src)227 cd_mat33_clear (const CdMat3x3 *src)
228 {
229 guint i;
230 gdouble *temp = (gdouble *) src;
231 for (i = 0; i < 3*3; i++)
232 temp[i] = 0.0f;
233 }
234
235 /**
236 * cd_mat33_to_string:
237 * @src: the source
238 *
239 * Obtains a string representaton of a matrix.
240 *
241 * Return value: the string. Free with g_free()
242 **/
243 gchar *
cd_mat33_to_string(const CdMat3x3 * src)244 cd_mat33_to_string (const CdMat3x3 *src)
245 {
246 return g_strdup_printf ("\n/ %0 .6f %0 .6f %0 .6f \\\n"
247 "| %0 .6f %0 .6f %0 .6f |\n"
248 "\\ %0 .6f %0 .6f %0 .6f /",
249 src->m00, src->m01, src->m02,
250 src->m10, src->m11, src->m12,
251 src->m20, src->m21, src->m22);
252 }
253
254 /**
255 * cd_mat33_get_data:
256 * @src: the matrix source
257 *
258 * Gets the raw data for the matrix.
259 *
260 * Return value: the pointer to the data segment.
261 **/
262 gdouble *
cd_mat33_get_data(const CdMat3x3 * src)263 cd_mat33_get_data (const CdMat3x3 *src)
264 {
265 return (gdouble *) src;
266 }
267
268 /**
269 * cd_mat33_set_identity:
270 * @src: the source
271 *
272 * Sets the matrix to an identity value.
273 **/
274 void
cd_mat33_set_identity(CdMat3x3 * src)275 cd_mat33_set_identity (CdMat3x3 *src)
276 {
277 cd_mat33_clear (src);
278 src->m00 = 1.0f;
279 src->m11 = 1.0f;
280 src->m22 = 1.0f;
281 }
282
283 /**
284 * cd_mat33_determinant:
285 * @src: the source
286 *
287 * Gets the determinant of the matrix.
288 **/
289 gdouble
cd_mat33_determinant(const CdMat3x3 * src)290 cd_mat33_determinant (const CdMat3x3 *src)
291 {
292 return src->m00 * src->m11 * src->m22 +
293 src->m01 * src->m12 * src->m20 +
294 src->m02 * src->m10 * src->m21 -
295 src->m02 * src->m11 * src->m20 -
296 src->m01 * src->m10 * src->m22 -
297 src->m00 * src->m12 * src->m21;
298 }
299
300 /**
301 * cd_mat33_normalize:
302 * @src: the source matrix
303 * @dest: the destination matrix
304 *
305 * Normalizes a matrix
306 *
307 * The arguments @src and @dest can be the same value.
308 **/
309 void
cd_mat33_normalize(const CdMat3x3 * src,CdMat3x3 * dest)310 cd_mat33_normalize (const CdMat3x3 *src, CdMat3x3 *dest)
311 {
312 gdouble *data_dest;
313 gdouble *data_src;
314 gdouble det;
315 guint i;
316
317 data_src = cd_mat33_get_data (src);
318 data_dest = cd_mat33_get_data (dest);
319 det = cd_mat33_determinant (src);
320 for (i = 0; i < 9; i++)
321 data_dest[i] = data_src[i] / det;
322 }
323
324
325 /**
326 * cd_mat33_vector_multiply:
327 * @mat_src: the matrix source
328 * @vec_src: the vector source
329 * @vec_dest: the destination vector
330 *
331 * Multiplies a matrix with a vector.
332 * The arguments @vec_src and @vec_dest cannot be the same value.
333 **/
334 void
cd_mat33_vector_multiply(const CdMat3x3 * mat_src,const CdVec3 * vec_src,CdVec3 * vec_dest)335 cd_mat33_vector_multiply (const CdMat3x3 *mat_src, const CdVec3 *vec_src, CdVec3 *vec_dest)
336 {
337 g_return_if_fail (vec_src != vec_dest);
338 vec_dest->v0 = mat_src->m00 * vec_src->v0 +
339 mat_src->m01 * vec_src->v1 +
340 mat_src->m02 * vec_src->v2;
341 vec_dest->v1 = mat_src->m10 * vec_src->v0 +
342 mat_src->m11 * vec_src->v1 +
343 mat_src->m12 * vec_src->v2;
344 vec_dest->v2 = mat_src->m20 * vec_src->v0 +
345 mat_src->m21 * vec_src->v1 +
346 mat_src->m22 * vec_src->v2;
347 }
348
349 /**
350 * cd_mat33_scalar_multiply:
351 * @mat_src: the source
352 * @value: the scalar
353 * @mat_dest: the destination
354 *
355 * Multiplies a matrix with a scalar.
356 * The arguments @vec_src and @vec_dest can be the same value.
357 **/
358 void
cd_mat33_scalar_multiply(const CdMat3x3 * mat_src,gdouble value,CdMat3x3 * mat_dest)359 cd_mat33_scalar_multiply (const CdMat3x3 *mat_src,
360 gdouble value,
361 CdMat3x3 *mat_dest)
362 {
363 gdouble *tmp_src;
364 gdouble *tmp_dest;
365 guint i;
366 tmp_src = cd_mat33_get_data (mat_src);
367 tmp_dest = cd_mat33_get_data (mat_dest);
368 for (i = 0; i < 9; i++)
369 tmp_dest[i] = tmp_src[i] * value;
370 }
371
372 /**
373 * cd_mat33_matrix_multiply:
374 * @mat_src1: the matrix source
375 * @mat_src2: the other matrix source
376 * @mat_dest: the destination
377 *
378 * Multiply (convolve) one matrix with another.
379 * The arguments @mat_src1 cannot be the same as @mat_dest, and
380 * @mat_src2 cannot be the same as @mat_dest.
381 **/
382 void
cd_mat33_matrix_multiply(const CdMat3x3 * mat_src1,const CdMat3x3 * mat_src2,CdMat3x3 * mat_dest)383 cd_mat33_matrix_multiply (const CdMat3x3 *mat_src1, const CdMat3x3 *mat_src2, CdMat3x3 *mat_dest)
384 {
385 guint8 i, j, k;
386 gdouble *src1 = cd_mat33_get_data (mat_src1);
387 gdouble *src2 = cd_mat33_get_data (mat_src2);
388 gdouble *dest = cd_mat33_get_data (mat_dest);
389 g_return_if_fail (mat_src1 != mat_dest);
390 g_return_if_fail (mat_src2 != mat_dest);
391
392 cd_mat33_clear (mat_dest);
393 for (i = 0; i < 3; i++) {
394 for (j = 0; j < 3; j++) {
395 for (k = 0; k < 3; k++) {
396 dest[3 * i + j] += src1[i * 3 + k] * src2[k * 3 + j];
397 }
398 }
399 }
400 }
401
402 /**
403 * cd_mat33_reciprocal:
404 * @src: the source
405 * @dest: the destination
406 *
407 * Inverts the matrix.
408 * The arguments @src and @dest cannot be the same value.
409 *
410 * Return value: %FALSE if det is zero (singular).
411 **/
412 gboolean
cd_mat33_reciprocal(const CdMat3x3 * src,CdMat3x3 * dest)413 cd_mat33_reciprocal (const CdMat3x3 *src, CdMat3x3 *dest)
414 {
415 double det = 0;
416
417 g_return_val_if_fail (src != dest, FALSE);
418
419 det = src->m00 * (src->m11 * src->m22 - src->m12 * src->m21);
420 det -= src->m01 * (src->m10 * src->m22 - src->m12 * src->m20);
421 det += src->m02 * (src->m10 * src->m21 - src->m11 * src->m20);
422
423 /* division by zero */
424 if (fabs (det) < 1e-6)
425 return FALSE;
426
427 dest->m00 = (src->m11 * src->m22 - src->m12 * src->m21) / det;
428 dest->m01 = (src->m02 * src->m21 - src->m01 * src->m22) / det;
429 dest->m02 = (src->m01 * src->m12 - src->m02 * src->m11) / det;
430
431 dest->m10 = (src->m12 * src->m20 - src->m10 * src->m22) / det;
432 dest->m11 = (src->m00 * src->m22 - src->m02 * src->m20) / det;
433 dest->m12 = (src->m02 * src->m10 - src->m00 * src->m12) / det;
434
435 dest->m20 = (src->m10 * src->m21 - src->m11 * src->m20) / det;
436 dest->m21 = (src->m01 * src->m20 - src->m00 * src->m21) / det;
437 dest->m22 = (src->m00 * src->m11 - src->m01 * src->m10) / det;
438
439 return TRUE;
440 }
441
442 /**
443 * cd_mat33_copy:
444 * @src: the source
445 * @dest: the destination
446 *
447 * Copies the matrix.
448 * The arguments @src and @dest cannot be the same value.
449 **/
450 void
cd_mat33_copy(const CdMat3x3 * src,CdMat3x3 * dest)451 cd_mat33_copy (const CdMat3x3 *src, CdMat3x3 *dest)
452 {
453 g_return_if_fail (src != dest);
454 memcpy (dest, src, sizeof (CdMat3x3));
455 }
456