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

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

added

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
19void 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.