starter/math/linear_algebra.cpp

   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
  29 /////////////////////////////////////////////////////////////////////////////////////////////////
  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 }
 121 /////////////////////////////////////////////////////////////////////////////////////////////////
 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
 170 /*!
 171   R = R(kappa) * R(phi) * R(omega)
 172   */
 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
 220 /////////////////////////////////////////////////////////////////////////////////////////////////
 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
 283 /////////////////////////////////////////////////////////////////////////////////////////////////
 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
 335 /////////////////////////////////////////////////////////////////////////////////////////////////
 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 }