Learn how easy it is to sync an existing GitHub or Google Code repo to a SourceForge project! See Demo

Close

[bf78f2]: src / libpanomatic / localfeatures / MathStuff.cpp Maximize Restore History

Download this file

MathStuff.cpp    101 lines (89 with data), 2.6 kB

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
/*
* Copyright (C) 2007-2008 Anael Orlinski
*
* This file is part of Panomatic.
*
* Panomatic is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* Panomatic is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Panomatic; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "MathStuff.h"
#include <math.h>
using namespace lfeat;
bool Math::SolveLinearSystem33(double *solution, double sq[3][3])
{
const int size = 3;
int row, col, c, pivot = 0, i;
double maxc, coef, temp, mult, val;
/* Triangularize the matrix. */
for (col = 0; col < size - 1; col++)
{
/* Pivot row with largest coefficient to top. */
maxc = -1.0;
for (row = col; row < size; row++)
{
coef = sq[row][col];
coef = (coef < 0.0 ? - coef : coef);
if (coef > maxc)
{
maxc = coef;
pivot = row;
}
}
if (pivot != col)
{
/* Exchange "pivot" with "col" row (this is no less efficient
than having to perform all array accesses indirectly). */
for (i = 0; i < size; i++)
{
temp = sq[pivot][i];
sq[pivot][i] = sq[col][i];
sq[col][i] = temp;
}
temp = solution[pivot];
solution[pivot] = solution[col];
solution[col] = temp;
}
/* Do reduction for this column. */
for (row = col + 1; row < size; row++)
{
mult = sq[row][col] / sq[col][col];
for (c = col; c < size; c++) /* Could start with c=col+1. */
sq[row][c] -= mult * sq[col][c];
solution[row] -= mult * solution[col];
}
}
/* Do back substitution. Pivoting does not affect solution order. */
for (row = size - 1; row >= 0; row--) {
val = solution[row];
for (col = size - 1; col > row; col--)
val -= solution[col] * sq[row][col];
solution[row] = val / sq[row][row];
}
return true;
}
bool Math::Normalize(double* iVec, int iLen)
{
int i;
double val, fac, sqlen = 0.0;
for (i = 0; i < iLen; i++) {
val = iVec[i];
sqlen += val * val;
}
if (sqlen == 0.0)
return false;
fac = 1.0 / sqrt(sqlen);
for (i = 0; i < iLen; i++)
iVec[i] *= fac;
return true;
}