#
source:
foam/trunk/vision/src/Matrix.cpp
@
98

Revision 98, 3.6 KB checked in by dave, 11 years ago (diff) |
---|

Line | |
---|---|

1 | // Copyright (C) 2009 foam |

2 | // |

3 | // This program is free software; you can redistribute it and/or modify |

4 | // it under the terms of the GNU General Public License as published by |

5 | // the Free Software Foundation; either version 2 of the License, or |

6 | // (at your option) any later version. |

7 | // |

8 | // This program is distributed in the hope that it will be useful, |

9 | // but WITHOUT ANY WARRANTY; without even the implied warranty of |

10 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |

11 | // GNU General Public License for more details. |

12 | // |

13 | // You should have received a copy of the GNU General Public License |

14 | // along with this program; if not, write to the Free Software |

15 | // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. |

16 | |

17 | #include "Matrix.h" |

18 | |

19 | void matrix_inverse(const float *Min, float *Mout, int actualsize) { |

20 | /* This function calculates the inverse of a square matrix |

21 | * |

22 | * matrix_inverse(float *Min, float *Mout, int actualsize) |

23 | * |

24 | * Min : Pointer to Input square float Matrix |

25 | * Mout : Pointer to Output (empty) memory space with size of Min |

26 | * actualsize : The number of rows/columns |

27 | * |

28 | * Notes: |

29 | * - the matrix must be invertible |

30 | * - there's no pivoting of rows or columns, hence, |

31 | * accuracy might not be adequate for your needs. |

32 | * |

33 | * Code is rewritten from c++ template code Mike Dinolfo |

34 | */ |

35 | /* Loop variables */ |

36 | int i, j, k; |

37 | /* Sum variables */ |

38 | float sum,x; |

39 | |

40 | /* Copy the input matrix to output matrix */ |

41 | for(i=0; i<actualsize*actualsize; i++) { Mout[i]=Min[i]; } |

42 | |

43 | /* Add small value to diagonal if diagonal is zero */ |

44 | for(i=0; i<actualsize; i++) |

45 | { |

46 | j=i*actualsize+i; |

47 | if((Mout[j]<1e-12)&&(Mout[j]>-1e-12)){ Mout[j]=1e-12; } |

48 | } |

49 | |

50 | /* Matrix size must be larger than one */ |

51 | if (actualsize <= 1) return; |

52 | |

53 | for (i=1; i < actualsize; i++) { |

54 | Mout[i] /= Mout[0]; /* normalize row 0 */ |

55 | } |

56 | |

57 | for (i=1; i < actualsize; i++) { |

58 | for (j=i; j < actualsize; j++) { /* do a column of L */ |

59 | sum = 0.0; |

60 | for (k = 0; k < i; k++) { |

61 | sum += Mout[j*actualsize+k] * Mout[k*actualsize+i]; |

62 | } |

63 | Mout[j*actualsize+i] -= sum; |

64 | } |

65 | if (i == actualsize-1) continue; |

66 | for (j=i+1; j < actualsize; j++) { /* do a row of U */ |

67 | sum = 0.0; |

68 | for (k = 0; k < i; k++) { |

69 | sum += Mout[i*actualsize+k]*Mout[k*actualsize+j]; |

70 | } |

71 | Mout[i*actualsize+j] = (Mout[i*actualsize+j]-sum) / Mout[i*actualsize+i]; |

72 | } |

73 | } |

74 | for ( i = 0; i < actualsize; i++ ) /* invert L */ { |

75 | for ( j = i; j < actualsize; j++ ) { |

76 | x = 1.0; |

77 | if ( i != j ) { |

78 | x = 0.0; |

79 | for ( k = i; k < j; k++ ) { |

80 | x -= Mout[j*actualsize+k]*Mout[k*actualsize+i]; |

81 | } |

82 | } |

83 | Mout[j*actualsize+i] = x / Mout[j*actualsize+j]; |

84 | } |

85 | } |

86 | for ( i = 0; i < actualsize; i++ ) /* invert U */ { |

87 | for ( j = i; j < actualsize; j++ ) { |

88 | if ( i == j ) continue; |

89 | sum = 0.0; |

90 | for ( k = i; k < j; k++ ) { |

91 | sum += Mout[k*actualsize+j]*( (i==k) ? 1.0 : Mout[i*actualsize+k] ); |

92 | } |

93 | Mout[i*actualsize+j] = -sum; |

94 | } |

95 | } |

96 | for ( i = 0; i < actualsize; i++ ) /* final inversion */ { |

97 | for ( j = 0; j < actualsize; j++ ) { |

98 | sum = 0.0; |

99 | for ( k = ((i>j)?i:j); k < actualsize; k++ ) { |

100 | sum += ((j==k)?1.0:Mout[j*actualsize+k])*Mout[k*actualsize+i]; |

101 | } |

102 | Mout[j*actualsize+i] = sum; |

103 | } |

104 | } |

105 | } |

**Note:**See TracBrowser for help on using the repository browser.