Flow123d  release_2.2.0-26-ge868538
math_fce.cc
Go to the documentation of this file.
1 /*!
2  *
3  * Copyright (C) 2015 Technical University of Liberec. All rights reserved.
4  *
5  * This program is free software; you can redistribute it and/or modify it under
6  * the terms of the GNU General Public License version 3 as published by the
7  * Free Software Foundation. (http://www.gnu.org/licenses/gpl-3.0.en.html)
8  *
9  * This program is distributed in the hope that it will be useful, but WITHOUT
10  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
11  * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
12  *
13  *
14  * @file math_fce.cc
15  * @ingroup la
16  * @brief Auxiliary math functions.
17  */
18 
19 #include <math.h>
20 #include <stdio.h> // for printf
21 #include <ostream> // for endl
22 #include "system/logger.hh" // for Logger, operator<<, Logger::MsgType, Log...
23 
24 #include "system/math_fce.h"
25 
26 
27 static double Inverse2(SmallMtx2 a,SmallMtx2 b);
28 static double Inverse3(SmallMtx3 a,SmallMtx3 b);
29 static double Inverse4(SmallMtx4 a,SmallMtx4 b);
30 
31 //=============================================================================
32 // LENGTH OF VECTOR
33 //=============================================================================
34 double vector_length( double v[] )
35 {
36  return sqrt( v[0]*v[0]+v[1]*v[1]+v[2]*v[2] );
37 }
38 //=============================================================================
39 // SCALAR PRODUCT OF VECTORS
40 //=============================================================================
41 double scalar_product( double u[], double v[] )
42 {
43  return u[0]*v[0]+u[1]*v[1]+u[2]*v[2];
44 }
45 //=============================================================================
46 // NORMALIZE GIVEN VECTOR
47 //=============================================================================
48 void normalize_vector( double u[] )
49 {
50  double l;
51 
52  if ((l = vector_length( u )) < NUM_ZERO ) {
53  WarningOut() << "Normalization of nearly zero vector." << std::endl;
54  }
55  u[0] /= l; u[1] /= l; u[2] /= l;
56 }
57 //=============================================================================
58 // MULTIPLE VECTOR BY REAL NUMBER
59 //=============================================================================
60 void scale_vector( double u[], double k )
61 {
62  u[ 0 ] *= k; u[ 1 ] *= k; u[ 2 ] *= k;
63 }
64 
65 //=============================================================================
66 // VECTOR PRODUCT OF VECTORS
67 //=============================================================================
68 void vector_product( double u[], double v[], double x[] )
69 {
70  // u, v - inputs
71  // x - output, result
72 
73  x[ 0 ] = u[ 1 ] * v[ 2 ] - u[ 2 ] * v[ 1 ];
74  x[ 1 ] = u[ 2 ] * v[ 0 ] - u[ 0 ] * v[ 2 ];
75  x[ 2 ] = u[ 0 ] * v[ 1 ] - u[ 1 ] * v[ 0 ];
76 }
77 //=============================================================================
78 // VECTOR DIFFERENCE OF VECTORS
79 //=============================================================================
80 void vector_difference( double u[], double v[], double x[] )
81 {
82  x[ 0 ] = u[ 0 ] - v[ 0 ];
83  x[ 1 ] = u[ 1 ] - v[ 1 ];
84  x[ 2 ] = u[ 2 ] - v[ 2 ];
85 }
86 
87 /*************************************************
88  * SMALL MATRIX OPERATIONS
89  *************************************************/
90 
91 /**************************************************
92  * determinant 3x3
93  * *************************/
94 double Det3( SmallMtx3 a ) {
95  return a[0][0]*SUBDET2(1,2,1,2)
96  -a[0][1]*SUBDET2(1,2,0,2)
97  +a[0][2]*SUBDET2(1,2,0,1);
98 }
99 
100 /*************************************************
101  * Inverse of general small matrix with given size
102  * a - input; b - output; returns determinant
103  * ************************************************/
104 double MatrixInverse(double *a, double *b, int size) {
105  switch (size) {
106  case 1:
107  *b=1/(*a); return (*a);
108  case 2:
109  return ( Inverse2((SmallMtx2)a, (SmallMtx2)b) );
110  case 3:
111  return ( Inverse3((SmallMtx3)a, (SmallMtx3)b) );
112  case 4:
113  return ( Inverse4((SmallMtx4)a, (SmallMtx4)b) );
114  }
115  return 0.0;
116 }
117 
118 
119 /******************************************************************
120  * Inverse of 2x2 matrix with determinant
121  * a - input; b - output; returns determinant;
122  * *****************************************************************/
124  double Det;
125 
126  Det=SUBDET2(0,1,0,1);
127  if ( fabs(Det) < NUM_ZERO ) return Det;
128  b[0][0]=a[1][1]/Det;
129  b[1][0]=-a[1][0]/Det;
130  b[0][1]=-a[0][1]/Det;
131  b[1][1]=a[0][0]/Det;
132  return Det;
133 }
134 
135 /******************************************************************
136  * Inverse of 3x3 matrix with determinant
137  * a - input; b - output; returns determinant;
138  * *****************************************************************/
139 
141  double Det;
142 
143  b[0][0]=SUBDET2(1,2,1,2);
144  b[1][0]=-SUBDET2(1,2,0,2);
145  b[2][0]=SUBDET2(1,2,0,1);
146 
147  Det=a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0];
148  if ( fabs(Det)< NUM_ZERO) return Det;
149  b[0][0]/=Det; b[1][0]/=Det; b[2][0]/=Det;
150 
151  b[0][1]=-SUBDET2(0,2,1,2)/Det;
152  b[1][1]=SUBDET2(0,2,0,2)/Det;
153  b[2][1]=-SUBDET2(0,2,0,1)/Det;
154 
155  b[0][2]=SUBDET2(0,1,1,2)/Det;
156  b[1][2]=-SUBDET2(0,1,0,2)/Det;
157  b[2][2]=SUBDET2(0,1,0,1)/Det;
158  return Det;
159 }
160 
161 /******************************************************************
162  * Inverse of 4x4 matrix with determinant
163  * a - input; b - output; det - determinant; returns determinant
164  * *****************************************************************/
165 
167  double u[6], l[6], Det;
168  // 2x2 subdets of rows 1,2
169  u[0]=SUBDET2(0,1,0,1);
170  u[1]=SUBDET2(0,1,0,2);
171  u[2]=SUBDET2(0,1,0,3);
172  u[3]=SUBDET2(0,1,1,2);
173  u[4]=SUBDET2(0,1,1,3);
174  u[5]=SUBDET2(0,1,2,3);
175 
176  //1x1 subdets of rows 2,3
177  l[0]=SUBDET2(2,3,0,1);
178  l[1]=SUBDET2(2,3,0,2);
179  l[2]=SUBDET2(2,3,0,3);
180  l[3]=SUBDET2(2,3,1,2);
181  l[4]=SUBDET2(2,3,1,3);
182  l[5]=SUBDET2(2,3,2,3);
183 
184  // 3x3 subdets
185  b[0][0]=+a[1][1]*l[5]-a[1][2]*l[4]+a[1][3]*l[3];
186  b[1][0]=-a[1][0]*l[5]+a[1][2]*l[2]-a[1][3]*l[1];
187  b[2][0]=+a[1][0]*l[4]-a[1][1]*l[2]+a[1][3]*l[0];
188  b[3][0]=-a[1][0]*l[3]+a[1][1]*l[1]-a[1][2]*l[0];
189  // 4x4 det
190  Det=a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0]+a[0][3]*b[3][0];
191  if ( fabs(Det)< NUM_ZERO) return Det;
192  b[0][0]/=Det; b[1][0]/=Det; b[2][0]/=Det; b[3][0]/=Det;
193 
194  b[0][1]=(-a[0][1]*l[5]+a[0][2]*l[4]-a[0][3]*l[3])/Det;
195  b[1][1]=(+a[0][0]*l[5]-a[0][2]*l[2]+a[0][3]*l[1])/Det;
196  b[2][1]=(-a[0][0]*l[4]+a[0][1]*l[2]-a[0][3]*l[0])/Det;
197  b[3][1]=(+a[0][0]*l[3]-a[0][1]*l[1]+a[0][2]*l[0])/Det;
198 
199  b[0][2]=(+a[3][1]*u[5]-a[3][2]*u[4]+a[3][3]*u[3])/Det;
200  b[1][2]=(-a[3][0]*u[5]+a[3][2]*u[2]-a[3][3]*u[1])/Det;
201  b[2][2]=(+a[3][0]*u[4]-a[3][1]*u[2]+a[3][3]*u[0])/Det;
202  b[3][2]=(-a[3][0]*u[3]+a[3][1]*u[1]-a[3][2]*u[0])/Det;
203 
204  b[0][3]=(-a[2][1]*u[5]+a[2][2]*u[4]-a[2][3]*u[3])/Det;
205  b[1][3]=(+a[2][0]*u[5]-a[2][2]*u[2]+a[2][3]*u[1])/Det;
206  b[2][3]=(-a[2][0]*u[4]+a[2][1]*u[2]-a[2][3]*u[0])/Det;
207  b[3][3]=(+a[2][0]*u[3]-a[2][1]*u[1]+a[2][2]*u[0])/Det;
208  return Det;
209 }
210 
211 /******************************************************************
212  * Print a full matrix stored in a vector size x size
213  * *****************************************************************/
214 void PrintSmallMatrix(double *mtx, int size ) {
215  int i,j;
216  printf("matrix %d x %d :\n",size,size);
217  for (i=0;i<size;i++) {
218  for(j=0;j<size;j++) printf("%f ",mtx[i*size+j]);
219  printf("\n");
220  }
221 }
222 
223 /*******************************************************************************
224  * at place inverse of an NxN matrix by Gauss-Jordan elimination without pivotation
225  * this should by faster then via adjung. matrix at least for N>3
226  * *****************************************************************************/
227 void MatInverse(int n,double **a) {
228  int row,subrow,col;
229  double Div,Mult;
230 
231  // forward run - lower triangle and diagonal of the inverse matrix
232  for(row=0;row<n-1;row++) {
233  // divide the row by diagonal
234  Div=a[row][row];a[row][row]=1;
235  for(col=0;col<n;col++) a[row][col]/=Div;
236  // eliminate the lower part of column "row"
237  for(subrow=row+1;subrow<n;subrow++) {
238  Mult=a[subrow][row];a[subrow][row]=0;
239  for(col=0;col<n;col++) a[subrow][col]-=a[row][col]*Mult;
240  }
241  }
242  // divide the last row
243  Div=a[row][row];a[row][row]=1;
244  for(col=0;col<n;col++) a[row][col]/=Div;
245  //backward run - upper trinagle
246  for(;row>0;row--) {
247  // eliminate the upper part of column "row"
248  for(subrow=row-1;subrow>=0;subrow--) {
249  Mult=a[subrow][row];a[subrow][row]=0;
250  for(col=0;col<n;col++) a[subrow][col]-=a[row][col]*Mult;
251  }
252  }
253 }
254 
255 //=============================================================================
256 // solve equation system - gauss; returns 1 - one result 0 - system doesn't have result; 2 - oo results
257 // !!! THIS FUNCTION IS NOT USED and probably is WRONG !!!
258 //=============================================================================
259 /*
260 int gauss(double *A, double *B,int s,double *R)
261 {
262  int res=1,i,j,k,size;
263  double koef,tmp,max;
264  double *M;
265  size=s+1;
266  M = (double *) xmalloc( (size-1) * size * sizeof(double));
267  for(i=0;i<s;i++)
268  {
269  for(j=0;j<s;j++)
270  M[i*(size)+j]=A[i*s+j];
271  M[i*(size)+j]=B[i];
272  }
273  for(i=0;i<size-1;i++)
274  {
275  if(M[i*size+i]==0)
276  {
277  for(j=i+1;j<size-1;j++)
278  {
279  if(M[j*size+i]!=0)
280  {
281  for(k=0;k<size;k++)
282  {
283  tmp=M[j*size+k];
284  M[j*size+k]=M[i*size+k];
285  M[i*size+k]=tmp;
286  }
287  j=size-1;
288  }
289  }
290  }
291  max=0;
292  for(k=0;k<size;k++)
293  if (fabs(max)<fabs(M[i*size+k])) max=fabs(M[i*size+k]);
294  if (max!=0)
295  for(k=0;k<size;k++)
296  M[i*size+k]/=max;
297  for(j=i+1;j<size-1;j++)
298  {
299  if (M[j*size+i]!=0)
300  {
301  koef=M[i*size+i]/M[j*size+i];
302  koef=fabs(koef);
303  if((M[i*size+i]>=0 && M[j*size+i]>=0) ||
304  (M[i*size+i]<0 && M[j*size+i]<0)) koef*=-1;
305  for(k=i;k<size;k++)
306  M[j*size+k]=M[j*size+k]*koef + M[i*size+k];
307 // M[j*size+i] = 0;
308  }
309  }
310  }
311  for(i=size-1-1;i>=0;i--)
312  {
313  koef=0;
314  for(j=i+1;j<size-1;j++)
315  {
316  koef=koef+R[j]*M[i*size+j];
317  }
318  if (!(DBL_EQ(M[i*size+i],0)))
319  R[i]=(M[i*size + size-1]-koef)/M[i*size+i];
320  else
321  {
322  if (koef==0 && M[i*size + size-1]==0)
323  return 2;
324  else
325  return 0;
326  }
327  }
328  xfree(M);
329  return res;
330 }
331 */
332 
333 //=============================================================================
334 // ANGLE BETWEEN TWO VECTORS
335 // !!! THIS FUNCTION IS NOT USED !!!
336 //=============================================================================
337 /*
338 double get_vectors_angle( double u[ 3 ], double v[ 3 ] )
339 {
340  double a,b,fi,p;
341  a = scalar_product (u, v);
342  b = vector_length( u ) * vector_length( v );
343  p = a / b;
344  if (p > 1) p = 1;
345  if (p < -1) p = -1;
346  fi = acos( p );
347  return fi * 180 / M_PI;
348 }
349 */
350 
351 //=============================================================================
352 // MATRIX TIMES MATRIX
353 // !!! THIS FUNCTION IS NOT USED !!!
354 //=============================================================================
355 /*
356 void matrix_x_matrix(double *A,int ra, int ca,
357  double *B, int rb, int cb, double *X)
358 {
359  int i,j,k;
360  OLD_ASSERT(!(ca != rb),"Matrix A has different number of columns than matrix B rows in the function matrix_x_matrix()\n");
361  for (i = 0; i < ra; i++)
362  for (j = 0; j < cb; j++)
363  {
364  X[ i* cb + j] = 0;
365  for (k = 0; k < rb; k++)
366  X[ i * cb + j] += A[ i * ca + k] * B[ k * cb + j];
367  }
368  return;
369 }
370 */
371 
372 //-----------------------------------------------------------------------------
373 // vim: set cindent:
void vector_product(double u[], double v[], double x[])
Definition: math_fce.cc:68
static double Inverse3(SmallMtx3 a, SmallMtx3 b)
Definition: math_fce.cc:140
#define SUBDET2(i, j, k, l)
Definition: math_fce.h:59
SmallVec3_t * SmallMtx3
Definition: math_fce.h:31
SmallVec4_t * SmallMtx4
Definition: math_fce.h:32
void normalize_vector(double u[])
Definition: math_fce.cc:48
double vector_length(double v[])
Definition: math_fce.cc:34
void scale_vector(double u[], double k)
Definition: math_fce.cc:60
static double Inverse2(SmallMtx2 a, SmallMtx2 b)
Definition: math_fce.cc:123
double scalar_product(double u[], double v[])
Definition: math_fce.cc:41
void MatInverse(int n, double **a)
Definition: math_fce.cc:227
void PrintSmallMatrix(double *mtx, int size)
Definition: math_fce.cc:214
#define NUM_ZERO
Numerical helpers.
Definition: math_fce.h:41
SmallVec2_t * SmallMtx2
Definition: math_fce.h:30
double Det3(SmallMtx3 a)
Definition: math_fce.cc:94
double MatrixInverse(double *a, double *b, int size)
Definition: math_fce.cc:104
void vector_difference(double u[], double v[], double x[])
Definition: math_fce.cc:80
#define WarningOut()
Macro defining &#39;warning&#39; record of log.
Definition: logger.hh:236
static double Inverse4(SmallMtx4 a, SmallMtx4 b)
Definition: math_fce.cc:166
void printf(BasicWriter< Char > &w, BasicCStringRef< Char > format, ArgList args)
Definition: printf.h:444