bazar  1.3.1
linear_algebra.cpp
Go to the documentation of this file.
1 /*
2 Copyright 2005, 2006 Computer Vision Lab,
3 Ecole Polytechnique Federale de Lausanne (EPFL), Switzerland.
4 All rights reserved.
5 
6 This file is part of BazAR.
7 
8 BazAR is free software; you can redistribute it and/or modify it under the
9 terms of the GNU General Public License as published by the Free Software
10 Foundation; either version 2 of the License, or (at your option) any later
11 version.
12 
13 BazAR is distributed in the hope that it will be useful, but WITHOUT ANY
14 WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
15 PARTICULAR PURPOSE. See the GNU General Public License for more details.
16 
17 You should have received a copy of the GNU General Public License along with
18 BazAR; if not, write to the Free Software Foundation, Inc., 51 Franklin
19 Street, Fifth Floor, Boston, MA 02110-1301, USA
20 */
21 #include <iostream>
22 #include <math.h>
23 
24 #include "linear_algebra.h"
25 #include <general/general.h>
26 
27 using namespace std;
28 
30 // Vectors:
31 
32 double gfla_norme(double v1, double v2, double v3)
33 {
34  return sqrt(v1 * v1 + v2 * v2 + v3 * v3);
35 }
36 
37 double gfla_normalize_3(double v[3])
38 {
39  double norme = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
40 
41  if (norme < 1e-10)
42  return 0;
43 
44  double inv_norme = 1. / norme;
45 
46  v[0] *= inv_norme;
47  v[1] *= inv_norme;
48  v[2] *= inv_norme;
49 
50  return norme;
51 }
52 
53 void gfla_scale_3(const double s, double v[3])
54 {
55  v[0] *= s;
56  v[1] *= s;
57  v[2] *= s;
58 }
59 
60 void gfla_scale_3(const double s, const double v[3], double sv[3])
61 {
62  sv[0] = s * v[0];
63  sv[1] = s * v[1];
64  sv[2] = s * v[2];
65 }
66 
67 void gfla_opp_3(double v[3])
68 {
69  v[0] = -v[0];
70  v[1] = -v[1];
71  v[2] = -v[2];
72 }
73 
74 void gfla_add_3(const double u[3], const double v[3], double w[3])
75 {
76  w[0] = u[0] + v[0];
77  w[1] = u[1] + v[1];
78  w[2] = u[2] + v[2];
79 }
80 
81 void gfla_sub_3(const double u[3], const double v[3], double w[3])
82 {
83  w[0] = u[0] - v[0];
84  w[1] = u[1] - v[1];
85  w[2] = u[2] - v[2];
86 }
87 
88 void gfla_cross_product(const double v1[3], const double v2[3], double v1v2[3])
89 {
90  v1v2[0] = v1[1] * v2[2] - v1[2] * v2[1];
91  v1v2[1] = v1[2] * v2[0] - v1[0] * v2[2];
92  v1v2[2] = v1[0] * v2[1] - v1[1] * v2[0];
93 }
94 
95 double gfla_dot_product(const double v1[3], const double v2[3])
96 {
97  return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
98 }
99 
100 
101 void gfla_copy_3(const double v[3], double copy[3])
102 {
103  for(int i = 0; i < 3; i++)
104  copy[i] = v[i];
105 }
106 
107 // Matrices:
108 void gfla_copy_3x3(const double M[3][3], double copy[3][3])
109 {
110  for(int i = 0; i < 3; i++)
111  for(int j = 0; j < 3; j++)
112  copy[i][j] = M[i][j];
113 }
114 
115 void gfla_copy_3x4(const double M[3][4], double copy[3][4])
116 {
117  for(int i = 0; i < 3; i++)
118  for(int j = 0; j < 4; j++)
119  copy[i][j] = M[i][j];
120 }
122 // Matrices:
123 
124 double gfla_det(const double M[3][3])
125 {
126  return M[0][0] * M[1][1] * M[2][2] -
127  M[0][0] * M[1][2] * M[2][1] -
128  M[1][0] * M[0][1] * M[2][2] +
129  M[1][0] * M[0][2] * M[2][1] +
130  M[2][0] * M[0][1] * M[1][2] -
131  M[2][0] * M[0][2] * M[1][1];
132 }
133 
134 double gfla_det(const double M11, const double M12,
135  const double M21, const double M22)
136 {
137  return M11 * M22 - M12 * M21;
138 }
139 
140 double gfla_det(const double M11, const double M12, const double M13,
141  const double M21, const double M22, const double M23,
142  const double M31, const double M32, const double M33)
143 {
144  return M11 * M22 * M33 - M11 * M23 * M32 -
145  M21 * M12 * M33 + M21 * M13 * M32 +
146  M31 * M12 * M23 - M31 * M13 * M22;
147 }
148 
149 void gfla_get_rotation_from_kappa(double R[3][3], const double kappa)
150 {
151  R[0][0] = cos(kappa); R[0][1] = sin(kappa); R[0][2] = 0.;
152  R[1][0] = -sin(kappa); R[1][1] = cos(kappa); R[1][2] = 0.;
153  R[2][0] = 0.; R[2][1] = 0.; R[2][2] = 1.;
154 }
155 
156 void gfla_get_rotation_from_phi(double R[3][3], const double phi)
157 {
158  R[0][0] = cos(phi); R[0][1] = 0.; R[0][2] = -sin(phi);
159  R[1][0] = 0.; R[1][1] = 1.; R[1][2] = 0.;
160  R[2][0] = sin(phi); R[2][1] = 0.; R[2][2] = cos(phi);
161 }
162 
163 void gfla_get_rotation_from_omega(double R[3][3], const double omega)
164 {
165  R[0][0] = 1.; R[0][1] = 0.; R[0][2] = 0.;
166  R[1][0] = 0.; R[1][1] = cos(omega); R[1][2] = sin(omega);
167  R[2][0] = 0.; R[2][1] = -sin(omega); R[2][2] = cos(omega);
168 }
169 
173 void gfla_get_rotation_from_euler_angles(double R[3][3], const double omega, const double phi, const double kappa)
174 {
175  double
176  t1 = sin(omega),
177  t2 = cos(phi),
178  t4 = cos(omega),
179  t6 = sin(kappa),
180  t8 = cos(kappa),
181  t10 = sin(phi),
182  t11 = t1*t10,
183  t15 = t4*t10;
184 
185  R[0][0] = t2*t8;
186  R[0][1] = t4*t6+t11*t8;
187  R[0][2] = t1*t6-t15*t8;
188  R[1][0] = -t2*t6;
189  R[1][1] = t4*t8-t11*t6;
190  R[1][2] = t1*t8+t15*t6;
191  R[2][0] = t10;
192  R[2][1] = -t1*t2;
193  R[2][2] = t4*t2;
194 }
195 
196 int gfla_get_euler_angles_from_rotation(const double R[3][3], double * omega, double * phi, double * kappa)
197 {
198  if (fabs(R[2][2]) < 1e-6 && fabs(R[2][1]) < 1e-6)
199  {
200  /* Degenerate case: When phi = +/- 90 degrees, then we cannot seperate omega and kappa */
201  *omega = atan2(R[1][2], R[1][1]);
202  if (R[2][0] > 0.)
203  *phi = M_PI/2;
204  else
205  *phi = -M_PI/2 ;
206  *kappa = 0.;
207 
208  return 0;
209  }
210  else
211  {
212  *omega = atan2(-R[2][1], R[2][2]);
213  *phi = atan2(R[2][0], sqrt(R[0][0] * R[0][0] + R[1][0] * R[1][0]));
214  *kappa = atan2(-R[1][0], R[0][0]);
215 
216  return 1;
217  }
218 }
219 
221 // Vectors/Matrices
222 
223 void gfla_mul_scale_mat_3x3(double s, double M[3][3], double sM[3][3])
224 {
225  sM[0][0] = s * M[0][0]; sM[0][1] = s * M[0][1]; sM[0][2] = s * M[0][2];
226  sM[1][0] = s * M[1][0]; sM[1][1] = s * M[1][1]; sM[1][2] = s * M[1][2];
227  sM[2][0] = s * M[2][0]; sM[2][1] = s * M[2][1]; sM[2][2] = s * M[2][2];
228 }
229 
230 void gfla_mul_mat_vect_3x3(const double M[3][3], const double v[3], double Mv[3])
231 {
232  if (v != Mv)
233  {
234  Mv[0] = M[0][0] * v[0] + M[0][1] * v[1] + M[0][2] * v[2];
235  Mv[1] = M[1][0] * v[0] + M[1][1] * v[1] + M[1][2] * v[2];
236  Mv[2] = M[2][0] * v[0] + M[2][1] * v[1] + M[2][2] * v[2];
237  }
238  else
239  {
240  double Mv_temp[3];
241 
242  Mv_temp[0] = M[0][0] * v[0] + M[0][1] * v[1] + M[0][2] * v[2];
243  Mv_temp[1] = M[1][0] * v[0] + M[1][1] * v[1] + M[1][2] * v[2];
244  Mv_temp[2] = M[2][0] * v[0] + M[2][1] * v[1] + M[2][2] * v[2];
245 
246  Mv[0] = Mv_temp[0];
247  Mv[1] = Mv_temp[1];
248  Mv[2] = Mv_temp[2];
249  }
250 }
251 
252 void gfla_mul_T_mat_vect_3x3(const double M[3][3], const double v[3], double tMv[3])
253 {
254  for(int i = 0; i < 3; i++)
255  {
256  tMv[i] = M[0][i] * v[0];
257  for(int j = 1; j < 3; j++)
258  tMv[i] += M[j][i] * v[j];
259  }
260 }
261 
262 void gfla_mul_mat_vect_3x4(const double M[3][4], const double v[3], double Mv[3])
263 {
264  for(int i = 0; i < 3; i++)
265  {
266  Mv[i] = M[i][0] * v[0];
267  for(int j = 1; j < 4; j++)
268  Mv[i] += M[i][j] * v[j];
269  }
270 }
271 
272 void gfla_mul_mat_3x3x4(const double M[3][3], const double N[3][4], double MN[3][4])
273 {
274  for(int i = 0; i < 3; i++)
275  for(int j = 0; j < 4; j++)
276  {
277  MN[i][j] = 0.;
278  for(int k = 0; k < 3; k++)
279  MN[i][j] += M[i][k] * N[k][j];
280  }
281 }
282 
284 // Inverse
285 
286 void gfla_inverse_3(double A[3][3], double invA[3][3])
287 {
288  double t1;
289  double t11;
290  double t13;
291  double t14;
292  double t15;
293  double t17;
294  double t18;
295  double t2;
296  double t20;
297  double t21;
298  double t23;
299  double t26;
300  double t4;
301  double t5;
302  double t8;
303  double t9;
304  {
305  t1 = A[1][1];
306  t2 = A[2][2];
307  t4 = A[1][2];
308  t5 = A[2][1];
309  t8 = A[0][0];
310 
311  t9 = t8*t1;
312  t11 = t8*t4;
313  t13 = A[1][0];
314  t14 = A[0][1];
315  t15 = t13*t14;
316  t17 = A[0][2];
317  t18 = t13*t17;
318  t20 = A[2][0];
319  t21 = t20*t14;
320  t23 = t20*t17;
321  t26 = 1/(t9*t2-t11*t5-t15*t2+t18*t5+t21*t4-t23*t1);
322  invA[0][0] = (t1*t2-t4*t5)*t26;
323  invA[0][1] = -(t14*t2-t17*t5)*t26;
324  invA[0][2] = -(-t14*t4+t17*t1)*t26;
325  invA[1][0] = -(t13*t2-t4*t20)*t26;
326  invA[1][1] = (t8*t2-t23)*t26;
327  invA[1][2] = -(t11-t18)*t26;
328  invA[2][0] = -(-t13*t5+t1*t20)*t26;
329  invA[2][1] = -(t8*t5-t21)*t26;
330  invA[2][2] = (t9-t15)*t26;
331  return;
332  }
333 }
334 
336 // Print:
337 
338 void gfla_print_mat_3x3(ostream & o, double M[3][3])
339 {
340  for(int i = 0; i < 3; i++)
341  {
342  for(int j = 0; j < 3; j++)
343  o << M[i][j] << "\t";
344  o << endl;
345  }
346 }
347 
348 void gfla_print_mat_3x4(ostream & o, double M[3][4])
349 {
350  for(int i = 0; i < 3; i++)
351  {
352  for(int j = 0; j < 4; j++)
353  o << M[i][j] << "\t";
354  o << endl;
355  }
356 }
357 
358 void gfla_print_mat_4x4(ostream & o, double * M)
359 {
360  for(int i = 0; i < 4; i++)
361  {
362  for(int j = 0; j < 4; j++)
363  o << M[4 * i + j] << "\t";
364  o << endl;
365  }
366 }
Generated on Tue Jan 15 2013 12:50:21 for bazar by   doxygen 1.8.1.2