source: ntrip/trunk/BNC/newmat/solution.cpp@ 9089

Last change on this file since 9089 was 8901, checked in by stuerze, 5 years ago

upgrade to newmat11 library

File size: 6.1 KB
Line 
1/// \ingroup newmat
2///@{
3
4/// \file solution.cpp
5/// One dimensional solve routine.
6
7// Copyright (C) 1994: R B Davies
8
9
10#define WANT_STREAM // include.h will get stream fns
11#define WANT_MATH // include.h will get math fns
12
13#include "include.h"
14#include "myexcept.h"
15
16#include "solution.h"
17
18#ifdef use_namespace
19namespace RBD_COMMON {
20#endif
21
22
23void R1_R1::Set(Real X)
24{
25 if ((!minXinf && X <= minX) || (!maxXinf && X >= maxX))
26 Throw(SolutionException("X value out of range"));
27 x = X; xSet = true;
28}
29
30R1_R1::operator Real()
31{
32 if (!xSet) Throw(SolutionException("Value of X not set"));
33 Real y = operator()();
34 return y;
35}
36
37unsigned long SolutionException::Select;
38
39SolutionException::SolutionException(const char* a_what) : BaseException()
40{
41 Select = BaseException::Select;
42 AddMessage("Error detected by solution package\n");
43 AddMessage(a_what); AddMessage("\n");
44 if (a_what) Tracer::AddTrace();
45}
46
47inline Real square(Real x) { return x*x; }
48
49void OneDimSolve::LookAt(int V)
50{
51 lim--;
52 if (!lim) Throw(SolutionException("Does not converge"));
53 Last = V;
54 Real yy = function(x[V]) - YY;
55 Finish = (fabs(yy) <= accY) || (Captured && fabs(x[L]-x[U]) <= accX );
56 y[V] = vpol*yy;
57}
58
59void OneDimSolve::HFlip() { hpol=-hpol; State(U,C,L); }
60
61void OneDimSolve::VFlip()
62 { vpol = -vpol; y[0] = -y[0]; y[1] = -y[1]; y[2] = -y[2]; }
63
64void OneDimSolve::Flip()
65{
66 hpol=-hpol; vpol=-vpol; State(U,C,L);
67 y[0] = -y[0]; y[1] = -y[1]; y[2] = -y[2];
68}
69
70void OneDimSolve::State(int I, int J, int K) { L=I; C=J; U=K; }
71
72void OneDimSolve::Linear(int I, int J, int K)
73{
74 x[J] = (x[I]*y[K] - x[K]*y[I])/(y[K] - y[I]);
75 // cout << "Linear\n";
76}
77
78void OneDimSolve::Quadratic(int I, int J, int K)
79{
80 // result to overwrite I
81 Real YJK, YIK, YIJ, XKI, XKJ;
82 YJK = y[J] - y[K]; YIK = y[I] - y[K]; YIJ = y[I] - y[J];
83 XKI = (x[K] - x[I]);
84 XKJ = (x[K]*y[J] - x[J]*y[K])/YJK;
85 if ( square(YJK/YIK)>(x[K] - x[J])/XKI ||
86 square(YIJ/YIK)>(x[J] - x[I])/XKI )
87 {
88 x[I] = XKJ;
89 // cout << "Quadratic - exceptional\n";
90 }
91 else
92 {
93 XKI = (x[K]*y[I] - x[I]*y[K])/YIK;
94 x[I] = (XKJ*y[I] - XKI*y[J])/YIJ;
95 // cout << "Quadratic - normal\n";
96 }
97}
98
99Real OneDimSolve::Solve(Real Y, Real X, Real Dev, int Lim)
100{
101 enum Loop { start, captured1, captured2, binary, finish };
102 Tracer et("OneDimSolve::Solve");
103 lim=Lim; Captured = false;
104 if ( Dev == 0.0 ) Throw(SolutionException("Dev is zero"));
105 L=0; C=1; U=2; vpol=1; hpol=1; y[C]=0.0; y[U]=0.0;
106 if (Dev<0.0) { hpol=-1; Dev = -Dev; }
107 YY=Y; // target value
108 x[L] = X; // initial trial value
109 if (!function.IsValid(X))
110 Throw(SolutionException("Starting value is invalid"));
111 Loop TheLoop = start;
112 for (;;)
113 {
114 switch (TheLoop)
115 {
116 case start:
117 LookAt(L); if (Finish) { TheLoop = finish; break; }
118 if (y[L]>0.0) VFlip(); // so Y[L] < 0
119
120 x[U] = X + Dev * hpol;
121 if (!function.maxXinf && x[U] > function.maxX)
122 x[U] = (function.maxX + X) / 2.0;
123 if (!function.minXinf && x[U] < function.minX)
124 x[U] = (function.minX + X) / 2.0;
125
126 LookAt(U); if (Finish) { TheLoop = finish; break; }
127 if (y[U] > 0.0) { TheLoop = captured1; Captured = true; break; }
128 if (y[U] == y[L])
129 Throw(SolutionException("Function is flat"));
130 if (y[U] < y[L]) HFlip(); // Change direction
131 State(L,U,C);
132 for (i=0; i<20; i++)
133 {
134 // cout << "Searching for crossing point\n";
135 // Have L C then crossing point, Y[L]<Y[C]<0
136 x[U] = x[C] + Dev * hpol;
137 if (!function.maxXinf && x[U] > function.maxX)
138 x[U] = (function.maxX + x[C]) / 2.0;
139 if (!function.minXinf && x[U] < function.minX)
140 x[U] = (function.minX + x[C]) / 2.0;
141
142 LookAt(U); if (Finish) { TheLoop = finish; break; }
143 if (y[U] > 0) { TheLoop = captured2; Captured = true; break; }
144 if (y[U] < y[C])
145 Throw(SolutionException("Function is not monotone"));
146 Dev *= 2.0;
147 State(C,U,L);
148 }
149 if (TheLoop != start ) break;
150 Throw(SolutionException("Cannot locate a crossing point"));
151
152 case captured1:
153 // cout << "Captured - 1\n";
154 // We have 2 points L and U with crossing between them
155 Linear(L,C,U); // linear interpolation
156 // - result to C
157 LookAt(C); if (Finish) { TheLoop = finish; break; }
158 if (y[C] > 0.0) Flip(); // Want y[C] < 0
159 if (y[C] < 0.5*y[L]) { State(C,L,U); TheLoop = binary; break; }
160
161 case captured2:
162 // cout << "Captured - 2\n";
163 // We have L,C before crossing, U after crossing
164 Quadratic(L,C,U); // quad interpolation
165 // - result to L
166 State(C,L,U);
167 if ((x[C] - x[L])*hpol <= 0.0 || (x[C] - x[U])*hpol >= 0.0)
168 { TheLoop = captured1; break; }
169 LookAt(C); if (Finish) { TheLoop = finish; break; }
170 // cout << "Through first stage\n";
171 if (y[C] > 0.0) Flip();
172 if (y[C] > 0.5*y[L]) { TheLoop = captured2; break; }
173 else { State(C,L,U); TheLoop = captured1; break; }
174
175 case binary:
176 // We have L, U around crossing - do binary search
177 // cout << "Binary\n";
178 for (i=3; i; i--)
179 {
180 x[C] = 0.5*(x[L]+x[U]);
181 LookAt(C); if (Finish) { TheLoop = finish; break; }
182 if (y[C]>0.0) State(L,U,C); else State(C,L,U);
183 }
184 if (TheLoop != binary) break;
185 TheLoop = captured1; break;
186
187 case finish:
188 return x[Last];
189
190 }
191 }
192}
193
194bool R1_R1::IsValid(Real X)
195{
196 Set(X);
197 return (minXinf || x > minX) && (maxXinf || x < maxX);
198}
199
200#ifdef use_namespace
201}
202#endif
203
204
205///@}
Note: See TracBrowser for help on using the repository browser.