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 |
---|
19 | namespace RBD_COMMON { |
---|
20 | #endif |
---|
21 | |
---|
22 | |
---|
23 | void 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 | |
---|
30 | R1_R1::operator Real() |
---|
31 | { |
---|
32 | if (!xSet) Throw(SolutionException("Value of X not set")); |
---|
33 | Real y = operator()(); |
---|
34 | return y; |
---|
35 | } |
---|
36 | |
---|
37 | unsigned long SolutionException::Select; |
---|
38 | |
---|
39 | SolutionException::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 | |
---|
47 | inline Real square(Real x) { return x*x; } |
---|
48 | |
---|
49 | void 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 | |
---|
59 | void OneDimSolve::HFlip() { hpol=-hpol; State(U,C,L); } |
---|
60 | |
---|
61 | void OneDimSolve::VFlip() |
---|
62 | { vpol = -vpol; y[0] = -y[0]; y[1] = -y[1]; y[2] = -y[2]; } |
---|
63 | |
---|
64 | void 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 | |
---|
70 | void OneDimSolve::State(int I, int J, int K) { L=I; C=J; U=K; } |
---|
71 | |
---|
72 | void 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 | |
---|
78 | void 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 | |
---|
99 | Real 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 | |
---|
194 | bool 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 | ///@} |
---|