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
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
|
//
// Big Vector and Sparse Matrix Classes
//
#include <float.h>
#include "vec3n.h"
float conjgrad_lasterror;
float conjgrad_epsilon = 0.1f;
int conjgrad_loopcount;
int conjgrad_looplimit = 100;
/*EXPORTVAR(conjgrad_lasterror);
EXPORTVAR(conjgrad_epsilon );
EXPORTVAR(conjgrad_loopcount);
EXPORTVAR(conjgrad_looplimit);
*/
int ConjGradient(float3N &X, float3Nx3N &A, float3N &B)
{
// Solves for unknown X in equation AX=B
conjgrad_loopcount=0;
int n=B.count;
float3N q(n),d(n),tmp(n),r(n);
r = B - Mul(tmp,A,X); // just use B if X known to be zero
d = r;
float s = dot(r,r);
float starget = s * squared(conjgrad_epsilon);
while( s>starget && conjgrad_loopcount++ < conjgrad_looplimit)
{
Mul(q,A,d); // q = A*d;
float a = s/dot(d,q);
X = X + d*a;
if(conjgrad_loopcount%50==0)
{
r = B - Mul(tmp,A,X);
}
else
{
r = r - q*a;
}
float s_prev = s;
s = dot(r,r);
d = r+d*(s/s_prev);
}
conjgrad_lasterror = s;
return conjgrad_loopcount<conjgrad_looplimit; // true means we reached desired accuracy in given time - ie stable
}
int ConjGradientMod(float3N &X, float3Nx3N &A, float3N &B,int3 hack)
{
// obsolete!!!
// Solves for unknown X in equation AX=B
conjgrad_loopcount=0;
int n=B.count;
float3N q(n),d(n),tmp(n),r(n);
r = B - Mul(tmp,A,X); // just use B if X known to be zero
r[hack[0]] = r[hack[1]] = r[hack[2]] = float3(0,0,0);
d = r;
float s = dot(r,r);
float starget = s * squared(conjgrad_epsilon);
while( s>starget && conjgrad_loopcount++ < conjgrad_looplimit)
{
Mul(q,A,d); // q = A*d;
q[hack[0]] = q[hack[1]] = q[hack[2]] = float3(0,0,0);
float a = s/dot(d,q);
X = X + d*a;
if(conjgrad_loopcount%50==0)
{
r = B - Mul(tmp,A,X);
r[hack[0]] = r[hack[1]] = r[hack[2]] = float3(0,0,0);
}
else
{
r = r - q*a;
}
float s_prev = s;
s = dot(r,r);
d = r+d*(s/s_prev);
d[hack[0]] = d[hack[1]] = d[hack[2]] = float3(0,0,0);
}
conjgrad_lasterror = s;
return conjgrad_loopcount<conjgrad_looplimit; // true means we reached desired accuracy in given time - ie stable
}
static inline void filter(float3N &V,const float3Nx3N &S)
{
for(int i=0;i<S.blocks.count;i++)
{
V[S.blocks[i].r] = V[S.blocks[i].r]*S.blocks[i].m;
}
}
int ConjGradientFiltered(float3N &X, const float3Nx3N &A, const float3N &B,const float3Nx3N &S)
{
// Solves for unknown X in equation AX=B
conjgrad_loopcount=0;
int n=B.count;
float3N q(n),d(n),tmp(n),r(n);
r = B - Mul(tmp,A,X); // just use B if X known to be zero
filter(r,S);
d = r;
float s = dot(r,r);
float starget = s * squared(conjgrad_epsilon);
while( s>starget && conjgrad_loopcount++ < conjgrad_looplimit)
{
Mul(q,A,d); // q = A*d;
filter(q,S);
float a = s/dot(d,q);
X = X + d*a;
if(conjgrad_loopcount%50==0)
{
r = B - Mul(tmp,A,X);
filter(r,S);
}
else
{
r = r - q*a;
}
float s_prev = s;
s = dot(r,r);
d = r+d*(s/s_prev);
filter(d,S);
}
conjgrad_lasterror = s;
return conjgrad_loopcount<conjgrad_looplimit; // true means we reached desired accuracy in given time - ie stable
}
// test big vector math library:
static void testfloat3N()
{
float3N a(2),b(2),c(2);
a[0] = float3(1,2,3);
a[1] = float3(4,5,6);
b[0] = float3(10,20,30);
b[1] = float3(40,50,60);
// c = a+b+b * 10.0f;
// float d = dot(a+b,-b);
int k;
k=0;
}
class dotest{public:dotest(){testfloat3N();}}do_test_at_program_startup;
|
