Marine systems simulation
PolynomialTools.h
1#pragma once
2
3#include <complex>
4
5namespace CoRiBoDynamics{
6
7namespace PolynomialTools{
8
12typedef std::complex<double> ComplexDouble;
13
14
27template <unsigned int N> class ComplexPolynomial
28{
29public:
34 void Evaluate(ComplexDouble x, ComplexDouble& fx, ComplexDouble& dfx, ComplexDouble& ddfx, double& err){
35 double abx = fabs(x);
36 err = fabs(C[N]);
37 fx = C[N];
38 dfx = 0.0;
39 ddfx = 0.0;
40 for(int i = N-1; i >= 0; i--){
41 ddfx = ddfx*x + dfx;
42 dfx = dfx*x + fx;
43 fx = fx*x + C[i];
44 err = err*abx + fabs(fx);
45 }
46 ddfx*=2.0;
47 err*=2.0*std::numeric_limits<double>::epsilon();
48 }
49
55 void ExtractRoot(ComplexDouble root, ComplexPolynomial<N-1>& Factor){
56 Factor.C[N-1] = C[N];
57 for(int i = N-2; i >= 0 ; i--){
58 Factor.C[i] = C[i+1] + Factor.C[i+1]*root;
59 }
60 }
61
65 ComplexDouble LaguerreIteration(const ComplexDouble& FX, const ComplexDouble& DFX, const ComplexDouble& DDFX){
66 ComplexDouble G = DFX/FX;
67 ComplexDouble H = G*G-DDFX/FX;
68 ComplexDouble SQ = sqrt(ComplexDouble(N-1)*(ComplexDouble(N)*H - G*G));
69 ComplexDouble GP = G + SQ;
70 ComplexDouble GM = G - SQ;
71 if(GM.real()*GM.real() + GM.imag()*GM.imag() > GP.real()*GP.real() + GP.imag()*GP.imag()){
72 GP = GM;
73 }
74 return ComplexDouble(N)/GP;
75 }
76
77
81 int FindRoots(ComplexDouble X0, ComplexDouble* roots){
82 ComplexDouble C_X, FX, DFX, DDFX;
83 double ERR;
84 C_X = X0;
85 Evaluate(X0,FX,DFX,DDFX,ERR);
86 int it = 0;
87 while(fabs(FX) > ERR){
88 C_X -= LaguerreIteration(FX, DFX, DDFX);
89 Evaluate(C_X,FX,DFX,DDFX,ERR);
90 it++;
91 }
92 roots[0] = C_X;
93 ComplexPolynomial<N-1> factorize;
94 ExtractRoot(C_X,factorize);
95 return it + factorize.FindRoots(X0,roots+1);
96 }
97
103 int FindRootsHQ(ComplexDouble X0, ComplexDouble* roots){
104 ComplexDouble C_X, FX, DFX, DDFX;
105 double ERR;
106 C_X = X0;
107 Evaluate(X0,FX,DFX,DDFX,ERR);
108 int it = 0;
109 while(fabs(FX) > ERR){
110 C_X -= LaguerreIteration(FX, DFX, DDFX);
111 Evaluate(C_X,FX,DFX,DDFX,ERR);
112 it++;
113 }
114 roots[0] = C_X;
115 ComplexPolynomial<N-1> factorize;
116 ExtractRoot(C_X,factorize);
117 int iterations = it + factorize.FindRootsHQ(X0,roots+1);
118 for(int i = 1; i < N; i++){
119 Evaluate(roots[i],FX,DFX,DDFX,ERR);
120 while(fabs(FX) > ERR){
121 roots[i] -= LaguerreIteration(FX, DFX, DDFX);
122 Evaluate(roots[i],FX,DFX,DDFX,ERR);
123 iterations++;
124 }
125 }
126 return iterations;
127 }
128 ComplexDouble C[N+1];
129};
130
131template <> class ComplexPolynomial<2>
132{
133public:
137 int FindRoots(ComplexDouble /*X0*/, ComplexDouble* roots){
138 ComplexDouble squareRoot = sqrt(C[1]*C[1]-4.0*C[2]*C[0]);
139 roots[0] = (( squareRoot - C[1])/(2.0*C[2]));
140 roots[1] = ((-squareRoot - C[1])/(2.0*C[2]));
141 return 0;
142 }
143 int FindRootsHQ(ComplexDouble /*X0*/, ComplexDouble* roots){
144 ComplexDouble squareRoot = sqrt(C[1]*C[1]-4.0*C[2]*C[0]);
145 roots[0] = (( squareRoot - C[1])/(2.0*C[2]));
146 roots[1] = ((-squareRoot - C[1])/(2.0*C[2]));
147 return 0;
148 }
149 ComplexDouble C[3];
150};
151
152template <unsigned int N> class RealPolynomial
153{
154public:
155 template <class Number> void Evaluate(Number x, Number& fx, Number& dfx, Number& ddfx, double& err){
156 double abx = fabs(x);
157 err = fabs(C[N]);
158 fx = C[N];
159 dfx = 0.0;
160 ddfx = 0.0;
161 for(int i = N-1; i >= 0; i--){
162 ddfx = ddfx*x + dfx;
163 dfx = dfx*x + fx;
164 fx = fx*x + C[i];
165 err = err*abx + fabs(fx);
166 }
167 ddfx*=2.0;
168 err*=2.0*std::numeric_limits<double>::epsilon();
169 }
170
171 void ExtractRealRoot(double root, RealPolynomial<N-1>& Factor){
172 Factor.C[N-1] = C[N];
173 for(int i = N-2; i >= 0 ; i--){
174 Factor.C[i] = C[i+1] + Factor.C[i+1]*root;
175 }
176 }
177
178 void ExtractComplexConjugateRootPair(ComplexDouble root, RealPolynomial<N-2>& Factor){
179 Factor.C[N-2] = C[N];
180 Factor.C[N-3] = C[N-1] + 2.0*root.real()*C[N];
181 for(int i = N-4; i >= 0 ; i--){
182 Factor.C[i] = C[i+2] + 2.0*Factor.C[i+1]*root.real() - (root.real()*root.real() + root.imag()*root.imag())*Factor.C[i+2];
183 }
184 }
185
186 int FindRoots(double X0, double* roots){
187 double FX, DFX, DDFX, ERR;
188 roots[0] = X0;
189 Evaluate(X0,FX,DFX,DDFX,ERR);
190 while(fabs(FX) > ERR){
191 double G = DFX/FX;
192 double H = G*G-DDFX/FX;
193 double discriminant = (N-1)*(N*H-G*G);
194 if(discriminant < 0){
195 ComplexDouble C_FX, C_DFX, C_DDFX;
196 ComplexDouble C_N = ComplexDouble(N);
197 ComplexDouble C_X = ComplexDouble(roots[0]) - (C_N/ComplexDouble(G,sqrt(fabs(discriminant))));
198
199 Evaluate(C_X,C_FX,C_DFX,C_DDFX,ERR);
200 while(fabs(C_FX) > ERR){
201 ComplexDouble C_G = C_DFX/C_FX;
202 ComplexDouble C_H = C_G*C_G-C_DDFX/C_FX;
203 ComplexDouble SQ = sqrt(ComplexDouble(N-1.0)*(C_N*C_H - C_G*C_G));
204
205 ComplexDouble GP = C_G + SQ;
206 ComplexDouble GM = C_G - SQ;
207 if(GM.real()*GM.real() + GM.imag()*GM.imag() > GP.real()*GP.real() + GP.imag()*GP.imag()){
208 GP = GM;
209 }
210 C_X -= C_N/GP;
211 Evaluate(C_X,C_FX,C_DFX,C_DDFX,ERR);
212 }
213 RealPolynomial<N-2> factorize;
214 ExtractComplexConjugateRootPair(C_X,factorize);
215 roots[0] = C_X.real();
216 roots[N-2] = std::numeric_limits<double>::quiet_NaN();
217 return 1+factorize.FindRoots(X0,roots+1);
218 }else{
219 roots[0] -= N/(G+(1-2*(G < 0))*sqrt(discriminant));
220 Evaluate(roots[0],FX,DFX,DDFX,ERR);
221 }
222 }
223 RealPolynomial<N-1> factorize;
224 ExtractRealRoot(roots[0],factorize);
225 return 1+factorize.FindRoots(roots[0],roots+1);
226 }
227 double C[N+1];
228};
229
230/*
231template <> class Polynom<0>
232{
233public:
234 int FindRoots(double X0, double* roots){roots[0] = std::numeric_limits<double>::quiet_NaN(); return 0;}
235 double C[1];
236};*/
237
238template <> class RealPolynomial<1>
239{
240public:
241 int FindRoots(double,double* roots){
242 if(C[1] != 0){
243 roots[0] = -C[0]/C[1];
244 return 1;
245 }else{
246 roots[0] = std::numeric_limits<double>::quiet_NaN();
247 return 0;
248 }
249 }
250 double C[2];
251};
252
253template <> class RealPolynomial<2>
254{
255public:
256 int FindRoots(double, double* roots){
257 double discriminant = C[1]*C[1]-4.0*C[2]*C[0];
258 if(discriminant >= 0){
259 double squareRoot = sqrt(discriminant);
260 roots[0] = ( squareRoot - C[1])/(2.0*C[2]);
261 roots[1] = (-squareRoot - C[1])/(2.0*C[2]);
262 return 2;
263 }else{
264 roots[0] = -0.5*C[1]/C[2];
265 roots[1] = std::numeric_limits<double>::quiet_NaN();
266 return 1;
267 }
268 }
269 double C[3];
270};
271
272}
273
274}
CoRiBoDynamics::PolynomialTools::ComplexPolynomial< 2 >::FindRoots
int FindRoots(ComplexDouble, ComplexDouble *roots)
Definition: PolynomialTools.h:137
CoRiBoDynamics::PolynomialTools::ComplexPolynomial
Class representing a general polynomial with complex coefficients.
Definition: PolynomialTools.h:28
CoRiBoDynamics::PolynomialTools::ComplexPolynomial::LaguerreIteration
ComplexDouble LaguerreIteration(const ComplexDouble &FX, const ComplexDouble &DFX, const ComplexDouble &DDFX)
Definition: PolynomialTools.h:65
CoRiBoDynamics::PolynomialTools::ComplexPolynomial::ExtractRoot
void ExtractRoot(ComplexDouble root, ComplexPolynomial< N-1 > &Factor)
Definition: PolynomialTools.h:55
CoRiBoDynamics::PolynomialTools::ComplexPolynomial::FindRootsHQ
int FindRootsHQ(ComplexDouble X0, ComplexDouble *roots)
Definition: PolynomialTools.h:103
CoRiBoDynamics::PolynomialTools::ComplexPolynomial::Evaluate
void Evaluate(ComplexDouble x, ComplexDouble &fx, ComplexDouble &dfx, ComplexDouble &ddfx, double &err)
Definition: PolynomialTools.h:34
CoRiBoDynamics::PolynomialTools::ComplexPolynomial::FindRoots
int FindRoots(ComplexDouble X0, ComplexDouble *roots)
Definition: PolynomialTools.h:81
CoRiBoDynamics::PolynomialTools::RealPolynomial
Definition: PolynomialTools.h:153
CoRiBoDynamics
Definition: CollisionManager.h:6