int main()
{
int n = 4; //order+1
double h = 0;
int maxiter = 100000;
double eps = pow(10,-10);
VEC A(n);
VEC R1(n-1);
/*VEC R2(n-1);
VEC R3(n-1);
VEC R4(n-1);*/
Complex a0(-6,0);
Complex a1(11,0);
Complex a2(-6,0);
Complex a3(1,0);
// Complex a4(0,0);
//Complex a5(0,0);
// Complex a6(1,0);
// Complex a7(1,0);
A[0] = a0; //x0
A[1] = a1; //x1
A[2] = a2; //x2
A[3] = a3;
// A[4] = a4;
// A[5] = a5;
// A[6] = a6;
// A[7] = a7;
for(int i=0;i<n;i++){
h = max(h,fabs(A[i]/A[n-1]));
}
Complex x1(1+h,1+h);
R1 = polyRoots(x1,A,maxiter,eps);
for(int i=0;i<n-1;i++){
printf("%lf + %lfj\n",R1[i].r(),R1[i].i());
}
return 0;
}
Real CylinderFit3<Real>::UpdateDirection (int numPoints,
const Vector3<Real>* points, const Vector3<Real>& center,
Vector3<Real>& axis, Real& invRSqr)
{
Real invNumPoints = ((Real)1)/(Real)numPoints;
int i;
Vector3<Real> delta, deltaCrossAxis, deltaCrossVDir;
Real a, b, c;
// Compute the direction of steepest descent.
Vector3<Real> vDir = Vector3<Real>::ZERO;
Real aMean = (Real)0, aaMean = (Real)0;
for (i = 0; i < numPoints; ++i)
{
delta = points[i] - center;
deltaCrossAxis = delta.Cross(axis);
a = invRSqr*deltaCrossAxis.SquaredLength() - (Real)1;
aMean += a;
aaMean += a*a;
vDir.X() += a*(axis.X()*(delta.Y()*delta.Y() +
delta.Z()*delta.Z()) - delta.X()*(axis.Y()*delta.Y() +
axis.Z()*delta.Z()));
vDir.Y() += a*(axis.Y()*(delta.X()*delta.X() +
delta.Z()*delta.Z()) - delta.Y()*(axis.X()*delta.X() +
axis.Z()*delta.Z()));
vDir.Z() += a*(axis.Z()*(delta.X()*delta.X() +
delta.Y()*delta.Y()) - delta.Z()*(axis.X()*delta.X() +
axis.Y()*delta.Y()));
}
aMean *= invNumPoints;
aaMean *= invNumPoints;
if (vDir.Normalize() < Math<Real>::ZERO_TOLERANCE)
{
return aaMean;
}
// Compute the 4th-degree polynomial for the line of steepest descent.
Real abMean = (Real)0, acMean = (Real)0;
Real bbMean = (Real)0, bcMean = (Real)0, ccMean = (Real)0;
for (i = 0; i < numPoints; ++i)
{
delta = points[i] - center;
deltaCrossAxis = delta.Cross(axis);
deltaCrossVDir = delta.Cross(vDir);
a = invRSqr*deltaCrossAxis.SquaredLength() - (Real)1;
b = invRSqr*(deltaCrossAxis.Dot(deltaCrossVDir));
c = invRSqr*deltaCrossVDir.SquaredLength();
abMean += a*b;
acMean += a*c;
bbMean += b*b;
bcMean += b*c;
ccMean += c*c;
}
abMean *= invNumPoints;
acMean *= invNumPoints;
bbMean *= invNumPoints;
bcMean *= invNumPoints;
ccMean *= invNumPoints;
Polynomial1<Real> poly(4);
poly[0] = aaMean;
poly[1] = -((Real)4)*abMean;
poly[2] = ((Real)2)*acMean + ((Real)4)*bbMean;
poly[3] = -((Real)4)*bcMean;
poly[4] = ccMean;
Polynomial1<Real> derPoly = poly.GetDerivative();
PolynomialRoots<Real> polyRoots(Math<Real>::ZERO_TOLERANCE);
polyRoots.FindA(derPoly[0], derPoly[1], derPoly[2], derPoly[3]);
int count = polyRoots.GetCount();
const Real* roots = polyRoots.GetRoots();
Real pMin = poly((Real)0);
int iMin = -1;
for (i = 0; i < count; ++i)
{
Real value = poly(roots[i]);
if (value < pMin)
{
pMin = value;
iMin = i;
}
}
if (iMin >= 0)
{
axis -= roots[iMin]*vDir;
Real length = axis.Normalize();
invRSqr *= length*length;
}
return pMin;
}
Real CylinderFit3<Real>::UpdateCenter (int numPoints,
const Vector3<Real>* points, Vector3<Real>& center,
const Vector3<Real>& axis, const Real& invRSqr)
{
Real invNumPoints = ((Real)1)/(Real)numPoints;
int i;
Vector3<Real> delta, deltaCrossAxis, cDirCrossAxis;
Real a, b, c;
// Compute the direction of steepest descent.
Vector3<Real> cDir = Vector3<Real>::ZERO;
Real aMean = (Real)0, aaMean = (Real)0;
for (i = 0; i < numPoints; ++i)
{
delta = points[i] - center;
deltaCrossAxis = delta.Cross(axis);
a = invRSqr*deltaCrossAxis.SquaredLength() - (Real)1;
aMean += a;
aaMean += a*a;
cDir += a*(delta - axis.Dot(delta)*axis); // |axis|=1 assumed
}
aMean *= invNumPoints;
aaMean *= invNumPoints;
if (cDir.Normalize() < Math<Real>::ZERO_TOLERANCE)
{
return aaMean;
}
// Compute the 4th-degree polynomial for the line of steepest descent.
cDirCrossAxis = cDir.Cross(axis);
c = cDirCrossAxis.SquaredLength()*invNumPoints*invRSqr;
Real bMean = (Real)0, abMean = (Real)0, bbMean = (Real)0;
for (i = 0; i < numPoints; ++i)
{
delta = points[i] - center;
deltaCrossAxis = delta.Cross(axis);
a = invRSqr*deltaCrossAxis.SquaredLength() - (Real)1;
b = invRSqr*(deltaCrossAxis.Dot(cDirCrossAxis));
bMean += b;
abMean += a*b;
bbMean += b*b;
}
bMean *= invNumPoints;
abMean *= invNumPoints;
bbMean *= invNumPoints;
Polynomial1<Real> poly(4);
poly[0] = aaMean;
poly[1] = ((Real)4)*abMean;
poly[2] = ((Real)2)*c*aMean + ((Real)4)*bbMean;
poly[3] = ((Real)4)*c*bMean;
poly[4] = c*c;
Polynomial1<Real> derPoly = poly.GetDerivative();
PolynomialRoots<Real> polyRoots(Math<Real>::ZERO_TOLERANCE);
polyRoots.FindA(derPoly[0], derPoly[1], derPoly[2], derPoly[3]);
int count = polyRoots.GetCount();
const Real* roots = polyRoots.GetRoots();
Real pMin = poly((Real)0);
int iMin = -1;
for (i = 0; i < count; ++i)
{
Real value = poly(roots[i]);
if (value < pMin)
{
pMin = value;
iMin = i;
}
}
if (iMin >= 0)
{
center -= roots[iMin]*cDir;
}
return pMin;
}
