3 template <
typename real>
10 template <
typename real>
17 template <
typename real>
27 template <
typename real>
35 roots.push_back (0.0);
39 roots.push_back (-b/a);
43 cout << __PRETTY_FUNCTION__ <<
": Found "<<roots.size ()<<
" roots.\n";
44 for (
unsigned int i=0; i<roots.size (); i++)
47 real result = a*x + b;
50 cout <<
"Something went wrong during solving of polynomial "<<a<<
"x + "<<b<<
" = 0\n";
53 cout <<
"Root "<<i<<
" = "<<roots[i]<<
". ("<<a<<
"x^ + "<<b<<
" = "<<result<<
")\n";
60 template <
typename real>
75 roots.push_back (0.0);
77 std::vector<real> tmpRoots;
79 for (
unsigned int i=0; i<tmpRoots.size (); i++)
81 roots.push_back (tmpRoots[i]);
85 real tmp = b*b - 4*a*c;
89 real tmp2 = 1.0/ (2*a);
90 roots.push_back ( (-b + tmp)*tmp2);
91 roots.push_back ( (-b - tmp)*tmp2);
95 roots.push_back (-b/ (2*a));
99 cout << __PRETTY_FUNCTION__ <<
": Found "<<roots.size ()<<
" roots.\n";
100 for (
unsigned int i=0; i<roots.size (); i++)
102 real x=roots[i], x2=x*x;
103 real result = a*x2 + b*x + c;
106 cout <<
"Something went wrong during solving of polynomial "<<a<<
"x^2 + "<<b<<
"x + "<<c<<
" = 0\n";
116 template<
typename real>
131 roots.push_back (0.0);
133 std::vector<real> tmpRoots;
135 for (
unsigned int i=0; i<tmpRoots.size (); i++)
137 roots.push_back (tmpRoots[i]);
145 alpha = ( (3.0*a*c-b2)/ (3.0*a2)),
146 beta = (2*b3/ (27.0*a3)) - ( (b*c)/ (3.0*a2)) + (d/a),
147 alpha2 = alpha*alpha,
148 alpha3 = alpha2*alpha,
152 double resubValue = b/ (3*a);
156 double discriminant = (alpha3/27.0) + 0.25*beta2;
164 roots.push_back ( (-3.0*beta)/ (2.0*alpha) - resubValue);
165 roots.push_back ( (3.0*beta)/alpha - resubValue);
169 roots.push_back (-resubValue);
172 else if (discriminant > 0)
174 double sqrtDiscriminant = sqrt (discriminant);
175 double d1 = -0.5*beta + sqrtDiscriminant,
176 d2 = -0.5*beta - sqrtDiscriminant;
178 d1 = -pow (-d1, 1.0/3.0);
180 d1 = pow (d1, 1.0/3.0);
183 d2 = -pow (-d2, 1.0/3.0);
185 d2 = pow (d2, 1.0/3.0);
188 roots.push_back (d1 + d2 - resubValue);
192 double tmp1 = sqrt (- (4.0/3.0)*alpha),
193 tmp2 = acos (-sqrt (-27.0/alpha3)*0.5*beta)/3.0;
194 roots.push_back (tmp1*cos (tmp2) - resubValue);
195 roots.push_back (-tmp1*cos (tmp2 + M_PI/3.0) - resubValue);
196 roots.push_back (-tmp1*cos (tmp2 - M_PI/3.0) - resubValue);
200 cout << __PRETTY_FUNCTION__ <<
": Found "<<roots.size ()<<
" roots.\n";
201 for (
unsigned int i=0; i<roots.size (); i++)
203 real x=roots[i], x2=x*x, x3=x2*x;
204 real result = a*x3 + b*x2 + c*x + d;
205 if (fabs (result) > 1e-4)
207 cout <<
"Something went wrong:\n";
210 cout <<
"Root "<<i<<
" = "<<roots[i]<<
". ("<<a<<
"x^3 + "<<b<<
"x^2 + "<<c<<
"x + "<<d<<
" = "<<result<<
")\n";
218 template<
typename real>
221 std::vector<real>& roots)
const
234 roots.push_back (0.0);
236 std::vector<real> tmpRoots;
238 for (
unsigned int i=0; i<tmpRoots.size (); i++)
240 roots.push_back (tmpRoots[i]);
244 double root1, root2, root3, root4,
251 alpha = ( (-3.0*b2)/ (8.0*a2)) + (c/a),
252 beta = (b3/ (8.0*a3)) - ( (b*c)/ (2.0*a2)) + (d/a),
253 gamma = ( (-3.0*b4)/ (256.0*a4)) + ( (c*b2)/ (16.0*a3)) - ( (b*d)/ (4.0*a2)) + (e/a),
254 alpha2 = alpha*alpha;
257 double resubValue = b/ (4*a);
264 std::vector<real> tmpRoots;
266 for (
unsigned int i=0; i<tmpRoots.size (); i++)
268 double qudraticRoot = tmpRoots[i];
271 roots.push_back (-resubValue);
273 else if (qudraticRoot > 0.0)
275 root1 = sqrt (qudraticRoot);
276 roots.push_back (root1 - resubValue);
277 roots.push_back (-root1 - resubValue);
284 double alpha3 = alpha2*alpha,
286 p = (-alpha2/12.0)-gamma,
287 q = (-alpha3/108.0)+ ( (alpha*gamma)/3.0)- (beta2/8.0),
290 u = (0.5*q) + sqrt ( (0.25*q2)+ (p3/27.0));
292 u = pow (u, 1.0/3.0);
296 u = -pow (-u, 1.0/3.0);
298 double y = (-5.0/6.0)*alpha - u;
302 double w = alpha + 2.0*y;
318 double tmp1 = - (3.0*alpha + 2.0*y + 2.0* (beta/w)),
319 tmp2 = - (3.0*alpha + 2.0*y - 2.0* (beta/w));
324 root1 = - (b/ (4.0*a)) + 0.5* (w+tmp1);
325 root2 = - (b/ (4.0*a)) + 0.5* (w-tmp1);
326 roots.push_back (root1);
327 roots.push_back (root2);
331 root1 = - (b/ (4.0*a)) + 0.5*w;
332 roots.push_back (root1);
338 root3 = - (b/ (4.0*a)) + 0.5* (-w+tmp2);
339 root4 = - (b/ (4.0*a)) + 0.5* (-w-tmp2);
340 roots.push_back (root3);
341 roots.push_back (root4);
345 root3 = - (b/ (4.0*a)) - 0.5*w;
346 roots.push_back (root3);
353 cout << __PRETTY_FUNCTION__ <<
": Found "<<roots.size ()<<
" roots.\n";
354 for (
unsigned int i=0; i<roots.size (); i++)
356 real x=roots[i], x2=x*x, x3=x2*x, x4=x2*x2;
357 real result = a*x4 + b*x3 + c*x2 + d*x + e;
358 if (fabs (result) > 1e-4)
360 cout <<
"Something went wrong:\n";
363 cout <<
"Root "<<i<<
" = "<<roots[i]
364 <<
". ("<<a<<
"x^4 + "<<b<<
"x^3 + "<<c<<
"x^2 + "<<d<<
"x + "<<e<<
" = "<<result<<
")\n";
372 template<
typename real>
375 std::vector<Eigen::Matrix<real, 3, 1> >& samplePoints,
unsigned int polynomial_degree,
bool& error)
const
384 template<
typename real>
387 std::vector<Eigen::Matrix<real, 3, 1> >& samplePoints,
unsigned int polynomial_degree,
397 if (parameters_size > samplePoints.size ())
410 Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> A (parameters_size, parameters_size);
412 Eigen::Matrix<real, Eigen::Dynamic, 1> b (parameters_size);
414 real currentX, currentY, currentZ;
416 real *tmpC =
new real[parameters_size];
417 real* tmpCEndPtr = &tmpC[parameters_size-1];
418 for (
typename std::vector<Eigen::Matrix<real, 3, 1> >::const_iterator it=samplePoints.begin ();
419 it!=samplePoints.end (); ++it)
421 currentX= (*it)[0]; currentY= (*it)[1]; currentZ= (*it)[2];
424 real* tmpCPtr = tmpCEndPtr;
426 for (
unsigned int xDegree=0; xDegree<=polynomial_degree; ++xDegree)
429 for (
unsigned int yDegree=0; yDegree<=polynomial_degree-xDegree; ++yDegree)
431 * (tmpCPtr--) = tmpX*tmpY;
438 real* APtr = &A(0,0);
441 for (
unsigned int i=0; i<parameters_size; ++i)
443 * (bPtr++) += currentZ * *tmpCPtr1;
446 for (
unsigned int j=0; j<parameters_size; ++j)
448 * (APtr++) += *tmpCPtr1 * * (tmpCPtr2++);
493 Eigen::Matrix<real, Eigen::Dynamic, 1> parameters;
499 parameters = A.inverse () * b;
504 real inversionCheckResult = (A*parameters - b).norm ();
505 if (inversionCheckResult > 1e-5)
511 for (
unsigned int i=0; i<parameters_size; i++)
real sqr_zero_value
sqr of the above
void setDegree(int new_degree)
Initialize members to default values.
bool isNearlyZero(real d) const
void solveLinearEquation(real a, real b, std::vector< real > &roots) const
Solves an equation of the form ax + b = 0.
PolynomialCalculationsT()
void setZeroValue(real new_zero_value)
Set zero_value.
bool sqrtIsNearlyZero(real d) const
void solveCubicEquation(real a, real b, real c, real d, std::vector< real > &roots) const
Solves an equation of the form ax^3 + bx^2 + cx + d = 0 See http://en.wikipedia.org/wiki/Cubic_equati...
void solveQuarticEquation(real a, real b, real c, real d, real e, std::vector< real > &roots) const
Solves an equation of the form ax^4 + bx^3 + cx^2 +dx + e = 0 See http://en.wikipedia.org/wiki/Quartic_equation#Summary_of_Ferrari.27s_method.
static unsigned int getNoOfParametersFromDegree(int n)
How many parametes has a bivariate polynomial of the given degree.
BivariatePolynomialT< real > bivariatePolynomialApproximation(std::vector< Eigen::Matrix< real, 3, 1 > > &samplePoints, unsigned int polynomial_degree, bool &error) const
Get the bivariate polynomial approximation for Z(X,Y) from the given sample points.
void solveQuadraticEquation(real a, real b, real c, std::vector< real > &roots) const
Solves an equation of the form ax^2 + bx + c = 0 See http://en.wikipedia.org/wiki/Quadratic_equation...
real zero_value
Every value below this is considered to be zero.
~PolynomialCalculationsT()
This represents a bivariate polynomial and provides some functionality for it.