Debian Codegraph @

plugins/core/Standard/qRANSAC_SD/extern/RANSAC_SD/solve.cpp

raw
1 #ifndef _USE_MATH_DEFINES
2 #define _USE_MATH_DEFINES
3
4 #include <cmath>
5 #include <GfxTL/MathHelper.h>
6
7 using namespace std;
8
9 int dmat_solve ( int n, int rhs_num, double a[] )
10 //******************************************************************************
11 //
12 // Purpose:
13 //
14 // DMAT_SOLVE uses Gauss-Jordan elimination to solve an N by N linear system.
15 //
16 // Discussion:
17 //
18 // The doubly dimensioned array A is treated as a one dimensional vector,
19 // stored by COLUMNS. Entry A(I,J) is stored as A[I+J*N]
20 //
21 // Modified:
22 //
23 // 29 August 2003
24 //
25 // Author:
26 //
27 // John Burkardt
28 //
29 // Parameters:
30 //
31 // Input, int N, the order of the matrix.
32 //
33 // Input, int RHS_NUM, the number of right hand sides. RHS_NUM
34 // must be at least 0.
35 //
36 // Input/output, double A[N*(N+RHS_NUM)], contains in rows and columns 1
37 // to N the coefficient matrix, and in columns N+1 through
38 // N+RHS_NUM, the right hand sides. On output, the coefficient matrix
39 // area has been destroyed, while the right hand sides have
40 // been overwritten with the corresponding solutions.
41 //
42 // Output, int DMAT_SOLVE, singularity flag.
43 // 0, the matrix was not singular, the solutions were computed;
44 // J, factorization failed on step J, and the solutions could not
45 // be computed.
46 //
47 {
48 double apivot;
49 double factor;
50 int i;
51 int ipivot;
52 int j;
53 int k;
54 double temp;
55
56 for ( j = 0; j < n; j++ )
57 {
58 //
59 // Choose a pivot row.
60 //
61 ipivot = j;
62 apivot = a[j+j*n];
63
64 for ( i = j; i < n; i++ )
65 {
66 if ( fabs ( apivot ) < fabs ( a[i+j*n] ) )
67 {
68 apivot = a[i+j*n];
69 ipivot = i;
70 }
71 }
72
73 if ( apivot == 0.0 )
74 {
75 return j;
76 }
77 //
78 // Interchange.
79 //
80 for ( i = 0; i < n + rhs_num; i++ )
81 {
82 temp = a[ipivot+i*n];
83 a[ipivot+i*n] = a[j+i*n];
84 a[j+i*n] = temp;
85 }
86 //
87 // A(J,J) becomes 1.
88 //
89 a[j+j*n] = 1.0;
90 for ( k = j; k < n + rhs_num; k++ )
91 {
92 a[j+k*n] = a[j+k*n] / apivot;
93 }
94 //
95 // A(I,J) becomes 0.
96 //
97 for ( i = 0; i < n; i++ )
98 {
99 if ( i != j )
100 {
101 factor = a[i+j*n];
102 a[i+j*n] = 0.0;
103 for ( k = j; k < n + rhs_num; k++ )
104 {
105 a[i+k*n] = a[i+k*n] - factor * a[j+k*n];
106 }
107 }
108
109 }
110
111 }
112
113 return 0;
114 }
115 //******************************************************************************
116 #endif