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; }